def rgtcapture(self):
"""
capture one goal pose using the rgt hndcam system
:return: a 4x4 h**o numpy mat
author: weiwei
date: 20180926
"""
# goalT = self.hc.getObjPoseRgtCam(self.rgtwatchpos, self.rgtwatchrotmat, marker_id=self.markerid)
virtualgoalpos0 = np.array([450, 350, 1200])
virtualgoalrot0 = rm.rodrigues([
1,
0,
0,
], 0)
virtualgoalpos1 = np.array([450, 250, 1200])
virtualgoalrot1 = rm.rodrigues([
1,
0,
0,
], 0)
virtualgoalpos2 = np.array([490, 250, 1200])
virtualgoalrot2 = rm.rodrigues([
1,
0,
0,
], 0)
virtualgoalpos3 = np.array([490, 350, 1200])
virtualgoalrot3 = rm.rodrigues([
1,
0,
0,
], 0)
goalTlist = [
rm.homobuild(virtualgoalpos0, virtualgoalrot0),
rm.homobuild(virtualgoalpos1, virtualgoalrot1),
rm.homobuild(virtualgoalpos2, virtualgoalrot2),
rm.homobuild(virtualgoalpos3, virtualgoalrot3)
]
ngoal = len(self.goallist)
if ngoal >= len(goalTlist):
return False
goalT = goalTlist[ngoal]
if goalT is None:
return False
else:
self.goallist.append(goalT)
tmpobjcm = copy.deepcopy(self.objcm)
tmpobjcm.setMat(base.pg.np4ToMat4(goalT))
tmpobjcm.reparentTo(base.render)
tmpobjcm.showLocalFrame()
self.objrenderlist.append(tmpobjcm)
for id, tmpobjcm in enumerate(self.objrenderlist):
tmpobjcm.setColor(0, (id) / float(len(self.objrenderlist)),
(id + 1) / float(len(self.objrenderlist)), 1)
return True
def rgtcapturestart(self):
"""
capture the starting pose using the rgt hndcam system
:return:
author: weiwei
date: 20180926
"""
# startT = None
# while startT is None:
# startT = self.hc.getObjPoseRgtCam(self.rgtwatchpos, self.rgtwatchrotmat, marker_id=self.markerid)
# startT = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[500,-350,1100,1.0]]).T
virtualgoalpos0 = np.array([500, -350, 1100])
virtualgoalrot0 = rm.rodrigues([
1,
0,
0,
], 0)
startT = rm.homobuild(virtualgoalpos0, virtualgoalrot0)
self.goallist = [startT] + self.goallist
self.startobjcm = copy.deepcopy(self.objcm)
self.startobjcm.setMat(base.pg.np4ToMat4(startT))
self.startobjcm.reparentTo(base.render)
self.startobjcm.showLocalFrame()
self.startobjcm.setColor(1, 0, 0, 1)
def trackobject_multicamfusion(camcaps, cammtxs, camdists, camrelhomos, aruco_dict, arucomarkersize = 100, nframe = 5, denoise = True, bandwidth=10):
"""
:param camcaps: a list of cv2.VideoCaptures
:param cammtxs: a list of mtx for each of the camcaps
:param camdists: as list of dist for each of the camcaps
:param camrelhomos: a list of relative homogeneous matrices
:param aruco_dict: NOTE this is not things like aruco.DICT_6x6_250, instead, it is the return value of aruco.Dictionary_get
:param nframe: number of frames used for fusion
:param denoise:
:param bandwidth: the bandwidth for meanshift, a large bandwidth leads to tracking instead of clustering, a small bandwith will be very costly
:return:
author: weiwei
date: 20190422
"""
parameters = aruco.DetectorParameters_create()
framelist = {}
for i in range(nframe):
for capid, cap in enumerate(camcaps):
ret, frame = cap.read()
corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, aruco_dict, parameters=parameters, ids=np.array([[1,2,3,4]]))
ids = ids.get()
if ids is not None:
rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corners, arucomarkersize, cammtxs[capid], camdists[capid])
for i in range(ids.size):
rot = cv2.Rodrigues(rvecs[i])[0]
pos = tvecs[i][0].ravel()
if capid > 0:
matinb = np.dot(rm.homoinverse(camrelhomos[capid-1]), rm.homobuild(pos, rot))
rot = matinb[:3, :3]
pos = matinb[:3, 3]
idslist = ids.ravel().tolist()
if idslist[i] in framelist:
framelist[idslist[i]].append([pos, rot])
else:
framelist[idslist[i]] = [[pos, rot]]
import time
frameavglist = {}
for id in framelist:
posveclist = [frame[0] for frame in framelist[id]]
rotmatlist = [frame[1] for frame in framelist[id]]
if len(posveclist) >= nframe:
posvecavg = rm.posvec_average(posveclist, bandwidth, denoise)
rotmatavg = rm.rotmat_average(rotmatlist, bandwidth, denoise)
frameavglist[id] = [posvecavg, rotmatavg]
return frameavglist
def rot_uv(uv, angle=30, toggledebug=False):
plt.scatter(np.asarray([v[0] for v in uv]),
np.asarray([v[1] for v in uv]),
color='red',
marker='.')
uv_3d = np.asarray([(v[0], v[1], 0) for v in uv])
uv_3d = pcdu.trans_pcd(
uv_3d,
rm.homobuild(np.asarray([0, 0, 0]), rm.rodrigues((0, 0, 1), angle)))
uv_new = np.asarray([(v[0], v[1]) for v in uv_3d])
if toggledebug:
plt.scatter(np.asarray([v[0] for v in uv_new]),
np.asarray([v[1] for v in uv_new]),
color='green',
marker='.')
plt.show()
return uv_new
def rgtcapture(self):
"""
capture one goal pose using the rgt hndcam system
:return: a 4x4 h**o numpy mat
author: weiwei
date: 20180926
"""
# goalT = self.hc.getObjPoseRgtCam(self.rgtwatchpos, self.rgtwatchrotmat, marker_id=self.markerid)
posG = base.pg.npToMat4(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
np.array([600, 0, 973 - 15]))
goalTlist = []
# plannedpath = readfile("Planned_pegpath_deriv1.txt")
plannedpath = readfile("Planned_pegpath_deriv2.txt")
for p in plannedpath:
relpos = np.array([p[0], p[1], p[2]])
relrot = rm.euler_matrix(p[3], p[4], p[5])
rel = base.pg.npToMat4(relrot, relpos)
posL = rel * posG
virtualgoalpos = np.array([posL[3][0], posL[3][1], posL[3][2]])
virtualgoalrot = np.array([[posL[0][0], posL[1][0], posL[2][0]],
[posL[0][1], posL[1][1], posL[2][1]],
[posL[0][2], posL[1][2], posL[2][2]]])
goalTlist.append(rm.homobuild(virtualgoalpos, virtualgoalrot))
ngoal = len(self.goallist)
if ngoal >= len(goalTlist):
return False
goalT = goalTlist[ngoal]
if goalT is None:
return False
else:
self.goallist.append(goalT)
tmpobjcm = copy.deepcopy(self.objcm)
tmpobjcm.setMat(base.pg.np4ToMat4(goalT))
tmpobjcm.reparentTo(base.render)
# tmpobjcm.showLocalFrame()
self.objrenderlist.append(tmpobjcm)
for id, tmpobjcm in enumerate(self.objrenderlist):
# tmpobjcm.setColor(0,(id)/float(len(self.objrenderlist)),(id+1)/float(len(self.objrenderlist)), 1)
tmpobjcm.setColor(1, 1, 0, 1 - id *0.01)
return True
def rgtcapture(self):
"""
capture one goal pose using the rgt hndcam system
:return: a 4x4 h**o numpy mat
author: weiwei
date: 20180926
"""
# goalT = self.hc.getObjPoseRgtCam(self.rgtwatchpos, self.rgtwatchrotmat, marker_id=self.markerid)
posG = base.pg.npToMat4(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
np.array([595, 0, 973 - 76 + 75 + 80]))
# print(posG)
relpos1 = np.array([-60.14147593, -0.43612779, 40.08426847])
relrot1 = np.array([[0.9672951, 0.003912, 0.25362363],
[-0.00265115, 0.99998247, -0.00531303],
[-0.25363992, 0.00446691, 0.9672884]])
rel1 = base.pg.npToMat4(relrot1, relpos1)
posL1 = rel1 * posG
virtualgoalpos0 = np.array([posL1[3][0], posL1[3][1], posL1[3][2]])
virtualgoalrot0 = np.array([[posL1[0][0], posL1[1][0], posL1[2][0]],
[posL1[0][1], posL1[1][1], posL1[2][1]],
[posL1[0][2], posL1[1][2], posL1[2][2]]])
relpos2 = np.array([-57.17716283, 0.1298598, 38.89503027])
relrot2 = np.array([[9.66031951e-01, -6.82621888e-03, 2.58332567e-01],
[7.04386040e-03, 9.99975300e-01, 8.30141152e-05],
[-2.58326703e-01, 1.73954899e-03, 9.66056106e-01]])
rel2 = base.pg.npToMat4(relrot2, relpos2)
posL2 = rel2 * posG
virtualgoalpos1 = np.array([posL2[3][0], posL2[3][1], posL2[3][2]])
virtualgoalrot1 = np.array([[posL2[0][0], posL2[1][0], posL2[2][0]],
[posL2[0][1], posL2[1][1], posL2[2][1]],
[posL2[0][2], posL2[1][2], posL2[2][2]]])
relpos3 = np.array([-54.82371822, -0.42677615, 37.11420346])
relrot3 = np.array([[0.974134, -0.01852004, 0.22521092],
[0.01522234, 0.99975027, 0.01637046],
[-0.22545782, -0.01251868, 0.97417256]])
rel3 = base.pg.npToMat4(relrot3, relpos3)
posL3 = rel3 * posG
virtualgoalpos2 = np.array([posL3[3][0], posL3[3][1], posL3[3][2]])
virtualgoalrot2 = np.array([[posL3[0][0], posL3[1][0], posL3[2][0]],
[posL3[0][1], posL3[1][1], posL3[2][1]],
[posL3[0][2], posL3[1][2], posL3[2][2]]])
relpos4 = np.array([-74.16420108, 0.46184687, 27.91712589])
relrot4 = np.array([[0.99992533, -0.00505429, -0.0111285],
[0.00514943, 0.99995039, 0.00853737],
[0.01108479, -0.00859397, 0.99990169]])
rel4 = base.pg.npToMat4(relrot4, relpos4)
posL4 = rel4 * posG
virtualgoalpos3 = np.array([posL4[3][0], posL4[3][1], posL4[3][2]])
virtualgoalrot3 = np.array([[posL4[0][0], posL4[1][0], posL4[2][0]],
[posL4[0][1], posL4[1][1], posL4[2][1]],
[posL4[0][2], posL4[1][2], posL4[2][2]]])
# virtualgoalpos0 = np.array([480,0,973-76+75+20])
# virtualgoalrot0 = rm.rodrigues([0,1,0,], 20)
# virtualgoalpos1 = np.array([450,250,1200])
# virtualgoalrot1 = rm.rodrigues([0,1,0,], -90)
# virtualgoalpos2 = np.array([490,250,1200])
# virtualgoalrot2 = rm.rodrigues([0,1,0,], -90)
# virtualgoalpos3 = np.array([490,350,1200])
# virtualgoalrot3 = rm.rodrigues([0,1,0,], -90)
goalTlist = [
rm.homobuild(virtualgoalpos0, virtualgoalrot0),
# rm.homobuild(virtualgoalpos1, virtualgoalrot1),
rm.homobuild(virtualgoalpos2, virtualgoalrot2),
rm.homobuild(virtualgoalpos3, virtualgoalrot3)
]
ngoal = len(self.goallist)
if ngoal >= len(goalTlist):
return False
goalT = goalTlist[ngoal]
if goalT is None:
return False
else:
self.goallist.append(goalT)
tmpobjcm = copy.deepcopy(self.objcm)
tmpobjcm.setMat(base.pg.np4ToMat4(goalT))
tmpobjcm.reparentTo(base.render)
tmpobjcm.showLocalFrame()
self.objrenderlist.append(tmpobjcm)
for id, tmpobjcm in enumerate(self.objrenderlist):
tmpobjcm.setColor(0, (id) / float(len(self.objrenderlist)),
(id + 1) / float(len(self.objrenderlist)), 1)
return True
fx_resample = ss.resample(np.array(fx), resample_num + del_num)
fy_resample = ss.resample(np.array(fy), resample_num + del_num)
fz_resample = ss.resample(np.array(fz), resample_num + del_num)
frx_resample = ss.resample(np.array(frx), resample_num + del_num)
fry_resample = ss.resample(np.array(fry), resample_num + del_num)
frz_resample = ss.resample(np.array(frz), resample_num + del_num)
# 计算物体在机器人坐标系下的位姿
pobj = []
robj = []
for i in range(resample_num + del_num):
rot_joint = rm.euler_matrix(prx_resample[i], pry_resample[i],
prz_resample[i])
pos_joint = np.array(
[px_resample[i], py_resample[i], pz_resample[i]])
joint_pose_to_base = rm.homobuild(pos_joint, rot_joint)
rot_obj0 = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])
pos_obj = np.array([0, 21, 54 + 15 + 130 + 15])
obj_pose_to_joint0 = rm.homobuild(pos_obj, rot_obj0)
obj_pose_to_joint = np.dot(
rm.homobuild(np.array([0, 0, 0]),
rm.euler_matrix(0, 0, 225 + 22.5)),
obj_pose_to_joint0)
obj_pose_to_base = np.dot(joint_pose_to_base, obj_pose_to_joint)
p_obj = obj_pose_to_base[:3, 3]
# print(p_obj)
pobj.append(p_obj)
rpy_obj = rm.euler_from_matrix(
pg.npToMat3(np.transpose(obj_pose_to_base[:3, :3])))
# print(rpy_obj)
robj.append(rpy_obj)
def buildCabinet(base,
cabpos=np.array([773.5, -55.17, 1035]),
cabrotangle=0,
isrendercoord=False):
__this_dir, _ = os.path.split(__file__)
cabinet = NodePath("cabinet")
#build the cabinet
#the middle board, 3 pieces
middleboardpath = os.path.join(__this_dir, "objects", "middleboard.stl")
middleboard0 = cm.CollisionModel(middleboardpath,
radius=3,
betransparency=True)
middleboard0.setColor(1, 0, 0, 1)
temprotmiddleboard = rm.rodrigues([0, 0, 1], 90)
rotmiddleboardmat4 = middleboard0.getMat()
rotmiddleboardnp4 = base.pg.mat4ToNp(rotmiddleboardmat4)
rotmiddleboardnp4[:3, :3] = np.dot(temprotmiddleboard,
rotmiddleboardnp4[:3, :3])
rotmiddleboardmat4 = base.pg.np4ToMat4(rotmiddleboardnp4)
middleboard0.setMat(rotmiddleboardmat4)
# temprotsmallboard = np.dot()
middleboard1 = copy.deepcopy(middleboard0)
middleboard1.setPos(0, 0, 288.5)
middleboard2 = copy.deepcopy(middleboard0)
temprotmiddleboard = rm.rodrigues([1, 0, 0], 180)
rotmiddleboardnp4[:3, :3] = np.dot(temprotmiddleboard,
rotmiddleboardnp4[:3, :3])
rotmiddleboardnp4[:3, 3] = np.array([0, 0, 587])
middleboard2.sethomomat(rotmiddleboardnp4)
# middleboard2.setPos(0,0,577)
middleboard0.setColor(.4, .4, .4, .7) #this 0 doesn't need to setpos
middleboard0.reparentTo(cabinet)
middleboard1.setColor(.4, .4, .4, .7)
middleboard1.reparentTo(cabinet)
middleboard2.setColor(.4, .4, .4, .7)
middleboard2.reparentTo(cabinet)
largeboardpath = os.path.join(__this_dir, "objects", "largeboard.stl")
largeboard0 = cm.CollisionModel(largeboardpath,
radius=3,
betransparency=True)
largeboard1 = copy.deepcopy(largeboard0)
largecompundrot0 = np.dot(rm.rodrigues([0, 1, 0], -90),
rm.rodrigues([1, 0, 0], 90))
largecompundrot1 = np.dot(rm.rodrigues([-1, 0, 0], 180), largecompundrot0)
largeboard0.sethomomat(
rm.homobuild(np.array([0, 195, 293.5]), largecompundrot0))
largeboard1.sethomomat(
rm.homobuild(np.array([0, -195, 293.5]), largecompundrot1))
largeboard0.setColor(.4, .4, .4, .7)
largeboard0.reparentTo(cabinet)
largeboard1.setColor(.4, .4, .4, .7)
largeboard1.reparentTo(cabinet)
# the small board, 2 pieces
smallboardpath = os.path.join(__this_dir, "objects", "smallboard.stl")
smallboard0 = cm.CollisionModel(smallboardpath,
radius=3,
betransparency=True)
smallboard1 = copy.deepcopy(smallboard0)
smallcompundrot = np.dot(rm.rodrigues([0, 1, 0], 90),
rm.rodrigues([0, 0, 1], -90))
smallboard0.sethomomat(
rm.homobuild(np.array([-142.5, 0, 149.25]), smallcompundrot))
smallboard1.sethomomat(
rm.homobuild(np.array([-142.5, 0, 587 - 149.25]), smallcompundrot))
smallboard0.setColor(.4, .4, .4, .7)
smallboard0.reparentTo(cabinet)
smallboard1.setColor(.4, .4, .4, .7)
smallboard1.reparentTo(cabinet)
if isrendercoord == True:
middleboard0.showLocalFrame()
middleboard1.showLocalFrame()
middleboard2.showLocalFrame()
largeboard0.showLocalFrame()
largeboard1.showLocalFrame()
smallboard0.showLocalFrame()
smallboard1.showLocalFrame()
#rotate the cabinet
cabinetassemblypos = cabpos # on the edge is 773, a bit outside is 814
cabinetassemblyrot = rm.rodrigues([0, 0, 1], cabrotangle)
cabinet.setMat(
base.pg.np4ToMat4(rm.homobuild(cabinetassemblypos,
cabinetassemblyrot)))
return cabinet
def phoxi_calib_refinewithmodel(yhx, pxc, rawamat, armname):
"""
The performance of this refining method using cad model is not good.
The reason is probably a precise mobdel is needed.
:param yhx: an instancde of YumiHelperX
:param pxc: phoxi client
:param armname:
:return:
author: weiwei
date: 20191228
"""
handpalmtemplate = pickle.load(
open(
os.path.join(yhx.root, "dataobjtemplate",
"handpalmtemplatepcd.pkl"), "rb"))
newhomomatlist = []
lastarmjnts = yhx.robot_s.initrgtjnts
eerot = np.array([[1, 0, 0], [0, 0, -1],
[0, 1, 0]]).T # horizontal, facing right
for x in [300, 360, 420]:
for y in range(-200, 201, 200):
for z in [70, 90, 130, 200]:
armjnts = yhx.movetoposrotmsc(eepos=np.array([x, y, z]),
eerot=eerot,
msc=lastarmjnts,
armname=armname)
if armjnts is not None and not yhx.pcdchecker.isSelfCollided(
yhx.robot_s):
lastarmjnts = armjnts
yhx.movetox(armjnts, armname=armname)
tcppos, tcprot = yhx.robot_s.gettcp()
initpos = tcppos + tcprot[:, 2] * 7
initrot = tcprot
inithomomat = rm.homobuild(initpos, initrot)
pxc.triggerframe()
pcd = pxc.getpcd()
realpcd = rm.homotransformpointarray(rawamat, pcd)
minx = tcppos[0] - 100
maxx = tcppos[0] + 100
miny = tcppos[1]
maxy = tcppos[1] + 140
minz = tcppos[2]
maxz = tcppos[2] + 70
realpcdcrop = o3dh.crop_nx3_nparray(
realpcd, [minx, maxx], [miny, maxy], [minz, maxz])
if len(realpcdcrop) < len(handpalmtemplate) / 2:
continue
# yhx.rbtmesh.genmnp(yhx.robot_s).reparentTo(base.render)
# yhx.p3dh.genframe(tcppos, tcprot, thickness=10). reparentTo(base.render)
# yhx.p3dh.gensphere([minx, miny, minz], radius=10).reparentTo(base.render)
# yhx.p3dh.gensphere([maxx, maxy, maxz], radius=10).reparentTo(base.render)
# yhx.p3dh.genpointcloudnodepath(realpcd).reparentTo(base.render)
# yhx.p3dh.genpointcloudnodepath(realpcdcrop, colors=[1,1,0,1]).reparentTo(base.render)
# yhx.p3dh.genpointcloudnodepath(rm.homotransformpointarray(inithomomat, handpalmtemplate), colors=[.5,1,.5,1]).reparentTo(base.render)
# yhx.base.run()
hto3d = o3dh.nparray_to_o3dpcd(
rm.homotransformpointarray(inithomomat,
handpalmtemplate))
rpo3d = o3dh.nparray_to_o3dpcd(realpcdcrop)
inlinnerrmse, newhomomat = o3dh.registration_icp_ptpt(
hto3d,
rpo3d,
np.eye(4),
maxcorrdist=2,
toggledebug=False)
print(inlinnerrmse, ", one round is done!")
newhomomatlist.append(rm.homoinverse(newhomomat))
newhomomat = rm.homomat_average(newhomomatlist, denoise=False)
refinedamat = np.dot(newhomomat, rawamat)
pickle.dump(
refinedamat,
open(os.path.join(yhx.root, "datacalibration", "refinedcalibmat.pkl"),
"wb"))
print(rawamat)
print(refinedamat)
return refinedamat
def find_rhomo(basecamyamlpath, relcamyamlpath, savename):
"""
compute the _rative transformation matrix (homogenous) between two cameras
the resulting matrix will be:
resulthomomat*p_in_rel=p_in_base
:param basecamyamlpath: results of board calibration, with (rvecs, tvecs, and candfiles) included
:param relcamyamlpath: results of board calibration, with (rvecs, tvecs, and candfiles) included
format of base and rel yaml: [mtx, dist, rvecs, tvecs, candfiles] see calibcharucoboard for example
:param savename:
:return:
author: weiwei
date: 20190421
"""
mtx_b, dist_b, rvecs_b, tvecs_b, candfiles_b = yaml.load(
open(basecamyamlpath, 'r'))
mtx_r, dist_r, rvecs_r, tvecs_r, candfiles_r = yaml.load(
open(relcamyamlpath, 'r'))
# get a list of relative mat from rel to base
homo_rb_list = []
for id_b, candimg_b in enumerate(candfiles_b):
try:
id_r = candfiles_r.index(candimg_b)
rot_b, _ = cv2.Rodrigues(rvecs_b[id_b].ravel())
pos_b = tvecs_b[id_b].ravel()
homo_b = rm.homobuild(pos_b, rot_b)
rot_r, _ = cv2.Rodrigues(rvecs_r[id_r].ravel())
pos_r = tvecs_r[id_r].ravel()
homo_r = rm.homobuild(pos_r, rot_r)
# homo_rb*homo_r = homo_b
homo_rb = np.dot(homo_b, rm.homoinverse(homo_r))
homo_rb_list.append(homo_rb)
except Exception as e:
print(e)
continue
# compute the average
pos_rb_list = []
quat_rb_list = []
angle_rb_list = []
for homo_rb in homo_rb_list:
quat_rb_list.append(rm.quaternion_from_matrix(homo_rb[:3, :3]))
angle_rb_list.append([rm.quaternion_angleaxis(quat_rb_list[-1])[0]])
pos_rb_list.append(homo_rb[:3, 3])
quat_rb_arr = np.array(quat_rb_list)
mt = cluster.MeanShift()
quat_rb_arrrefined = quat_rb_arr[np.where(
mt.fit(angle_rb_list).labels_ == 0)]
quat_rb_avg = rm.quaternion_average(quat_rb_arrrefined)
pos_rb_avg = mt.fit(pos_rb_list).cluster_centers_[0]
homo_rb_avg = rm.quaternion_matrix(quat_rb_avg)
homo_rb_avg[:3, 3] = pos_rb_avg
with open(savename, "w") as f:
yaml.dump(rm.homoinverse(homo_rb_avg), f)
# calibcharucoboard(7,5, squaremarkersize=squaremarkersize, imgspath='./camimgs4/', savename='cam4_calib.yaml')
arucomarkersize = int(40 * .57)
# find_rhomo(basecamyamlpath = 'cam0_calib.yaml', relcamyamlpath = 'cam2_calib.yaml', savename = 'homo_rb20.yaml')
# find_rhomo(basecamyamlpath = 'cam0_calib.yaml', relcamyamlpath = 'cam4_calib.yaml', savename = 'homo_rb40.yaml')
import math
homo_rb20 = yaml.load(open('homo_rb20.yaml', 'r'))
homo_rb40 = yaml.load(open('homo_rb40.yaml', 'r'))
# draw in 3d to validate
from pandaplotutils import pandactrl
base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 0])
homo_b = rm.homobuild(pos=np.ones(3), rot=np.eye(3))
base.pggen.plotAxis(base.render)
pandamat4homo_r2 = base.pg.np4ToMat4(rm.homoinverse(homo_rb20))
base.pggen.plotAxis(base.render,
spos=pandamat4homo_r2.getRow3(3),
pandamat3=pandamat4homo_r2.getUpper3())
pandamat4homo_r4 = base.pg.np4ToMat4(rm.homoinverse(homo_rb40))
base.pggen.plotAxis(base.render,
spos=pandamat4homo_r4.getRow3(3),
pandamat3=pandamat4homo_r4.getUpper3())
# base.run()
# show in videos
mtx0, dist0, rvecs0, tvecs0, candfiles0 = yaml.load(
open('cam0_calib.yaml', 'r'))
radius=3,
betransparency=True)
middleboard0 = cm.CollisionModel(middleboardpath,
radius=3,
betransparency=True)
smallboard0 = cm.CollisionModel(smallboardpath,
radius=3,
betransparency=True)
# the obstacle boards
largeboardobstacle0 = copy.deepcopy(largeboard0)
largecompundrot = np.dot(rm.rodrigues([0, 1, 0], -90),
rm.rodrigues([1, 0, 0], 90))
largeboardobstacle0.sethomomat(
rm.homobuild(
np.array([0, 195, 293.5]) + cabinetposenp4[:3, 3],
largecompundrot))
largeboardobstacle0.setColor(0, 0, 0, 1)
# largeboardobstacle0.reparentTo(base.render)
smallboardobstacle0 = copy.deepcopy(smallboard0)
smallboardobstacle1 = copy.deepcopy(smallboard0)
smallcompundrot = np.dot(rm.rodrigues([0, 1, 0], 90),
rm.rodrigues([0, 0, 1], -90))
smallboardobstacle0.sethomomat(
rm.homobuild(
np.array([-142.5, 0, 149.25]) + cabinetposenp4[:3, 3],
smallcompundrot))
smallboardobstacle1.sethomomat(
rm.homobuild(
def setangles(self, angle_main, angle_finger1, angle_finger2,
angle_finger3):
"""
set the 4 motor angles
angle_main = palm joint
angle_finger1 = index
angle_finger2 = middle
angle_finger3 = thumb
:param angle_main: degree
:param angle_finger1:
:param angle_finger2:
:param angle_finger3:
:return:
author: weiwei
date: 20200322
"""
if angle_main < self.__am_range["rngmin"]:
print("Warning. Range of angle_main must be larger than " +
str(self.__am_range["rngmin"]))
angle_main = self.__am_range["rngmin"]
elif angle_main > self.__am_range["rngmax"]:
print("Warning. Range of angle_main must be smaller than " +
str(self.__am_range["rngmax"]))
angle_main = self.__am_range["rngmax"]
if angle_finger1 < self.__af1_range["rngmin"]:
print("Warning. Range of angle_finger1 must be larger than " +
str(self.__af1_range["rngmin"]))
angle_finger1 = self.__af1_range["rngmin"]
elif angle_finger1 > self.__af1_range["rngmax"]:
print("Warning. Range of angle_finger1 must be smaller than " +
str(self.__af1_range["rngmax"]))
angle_finger1 = self.__af1_range["rngmax"]
if angle_finger2 < self.__af2_range["rngmin"]:
print("Warning. Range of angle_finger2 must be larger than " +
str(self.__af2_range["rngmin"]))
angle_finger2 = self.__af2_range["rngmin"]
elif angle_finger2 > self.__af2_range["rngmax"]:
print("Warning. Range of angle_finger2 must be smaller than " +
str(self.__af2_range["rngmax"]))
angle_finger2 = self.__af2_range["rngmax"]
if angle_finger3 < self.__af3_range["rngmin"]:
print("Warning. Range of angle_finger3 must be larger than " +
str(self.__af3_range["rngmin"]))
angle_finger3 = self.__af3_range["rngmin"]
elif angle_finger3 > self.__af3_range["rngmax"]:
print("Warning. Range of angle_finger3 must be smaller than " +
str(self.__af3_range["rngmax"]))
angle_finger3 = self.__af3_range["rngmax"]
self.__am_range = {"rngmin": 0, "rngmax": 180}
self.__angle_finger1 = {"rngmin": 0, "rngmax": 180}
self.__angle_finger2 = {"rngmin": 0, "rngmax": 140}
self.__angle_main = angle_main
self.__angle_finger1 = angle_finger1
self.__angle_finger2 = angle_finger2
self.__angle_finger3 = angle_finger3
# update finger positions
# finger 1
finger1_prox_pos = self.__finger1_prox_pos
finger1_prox_rot = np.dot(
self.__finger1_prox_rot,
rm.rodrigues(np.array([0, 0, 1]), -self.__angle_main))
self.__finger1_prox.sethomomat(
rm.homobuild(finger1_prox_pos, finger1_prox_rot))
finger1_med_pos = finger1_prox_pos + np.dot(finger1_prox_rot,
self.__finger1_med_pos)
finger1_med_rot = np.dot(
finger1_prox_rot,
np.dot(self.__finger1_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger1)))
self.__finger1_med.sethomomat(
rm.homobuild(finger1_med_pos, finger1_med_rot))
finger1_dist_pos = finger1_med_pos + np.dot(finger1_med_rot,
self.__finger1_dist_pos)
finger1_dist_rot = np.dot(
finger1_med_rot,
np.dot(self.__finger1_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger1 / 3)))
self.__finger1_dist.sethomomat(
rm.homobuild(finger1_dist_pos, finger1_dist_rot))
self.__finger1_prox.reparentTo(self.__hndbase)
self.__finger1_med.reparentTo(self.__hndbase)
self.__finger1_dist.reparentTo(self.__hndbase)
# finger 2
finger2_prox_pos = self.__finger2_prox_pos
finger2_prox_rot = np.dot(
self.__finger2_prox_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_main))
self.__finger2_prox.sethomomat(
rm.homobuild(finger2_prox_pos, finger2_prox_rot))
finger2_med_pos = finger2_prox_pos + np.dot(finger2_prox_rot,
self.__finger2_med_pos)
finger2_med_rot = np.dot(
finger2_prox_rot,
np.dot(self.__finger2_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger2)))
self.__finger2_med.sethomomat(
rm.homobuild(finger2_med_pos, finger2_med_rot))
finger2_dist_pos = finger2_med_pos + np.dot(finger2_med_rot,
self.__finger2_dist_pos)
finger2_dist_rot = np.dot(
finger2_med_rot,
np.dot(self.__finger2_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger2 / 3)))
self.__finger2_dist.sethomomat(
rm.homobuild(finger2_dist_pos, finger2_dist_rot))
self.__finger2_prox.reparentTo(self.__hndbase)
self.__finger2_med.reparentTo(self.__hndbase)
self.__finger2_dist.reparentTo(self.__hndbase)
# finger 3
finger3_med_pos = self.__finger3_med_pos
finger3_med_rot = np.dot(
self.__finger3_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger3))
self.__finger3_med.sethomomat(
rm.homobuild(finger3_med_pos, finger3_med_rot))
finger3_dist_pos = finger3_med_pos + np.dot(finger3_med_rot,
self.__finger3_dist_pos)
finger3_dist_rot = np.dot(
finger3_med_rot,
np.dot(self.__finger3_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger3 / 3)))
self.__finger3_dist.sethomomat(
rm.homobuild(finger3_dist_pos, finger3_dist_rot))
#
self.__finger3_med.reparentTo(self.__hndbase)
self.__finger3_dist.reparentTo(self.__hndbase)
self.__hndbase.reparentTo(self.__hndnp)
def __init__(self, *args, **kwargs):
"""
load the robotiq85 model, set jaw_width
:param args:
:param kwargs:
'jaw_width' 0-85
'ftsensoroffset' the offset for forcesensor
'toggleframes' True, False
author: weiwei
date: 20160627, 20190518, 20190824osaka, 20200321osaka
"""
if 'jawwidthopen' in kwargs:
self.__jawwidthopen = kwargs['jawwidthopen']
else:
self.__jawwidthopen = 100
if 'jawwidthclose' in kwargs:
self.__jawwidthclose = kwargs['jawwidthclose']
else:
self.__jawwidthclose = 0
if 'jaw_width' in kwargs:
self.__jawwidth = kwargs['jaw_width']
else:
self.__jawwidth = self.__jawwidthopen
if 'ftsensoroffset' in kwargs:
self.__ftsensoroffset = kwargs['ftsensoroffset']
if 'toggleframes' in kwargs:
self.__toggleframes = kwargs['toggleframes']
if 'hndbase' in kwargs:
self.__hndbase = cp.deepcopy(kwargs['hndbase'])
# for copy
self.__hndbase_bk = cp.deepcopy(kwargs['hndbase'])
if 'hndfingerprox' in kwargs:
self.__finger1_prox = cp.deepcopy(kwargs['hndfingerprox'])
self.__finger2_prox = cp.deepcopy(kwargs['hndfingerprox'])
self.__finger3_prox = cp.deepcopy(kwargs['hndfingerprox'])
# for copy
self.__hndfingerprox_bk = cp.deepcopy(kwargs['hndfingerprox'])
if 'hndfingermed' in kwargs:
self.__finger1_med = cp.deepcopy(kwargs['hndfingermed'])
self.__finger2_med = cp.deepcopy(kwargs['hndfingermed'])
self.__finger3_med = cp.deepcopy(kwargs['hndfingermed'])
# for copy
self.__hndfingermed_bk = cp.deepcopy(kwargs['hndfingermed'])
if 'hndfingerdist' in kwargs:
self.__finger1_dist = cp.deepcopy(kwargs['hndfingerdist'])
self.__finger2_dist = cp.deepcopy(kwargs['hndfingerdist'])
self.__finger3_dist = cp.deepcopy(kwargs['hndfingerdist'])
# for copy
self.__hndfingerdist_bk = cp.deepcopy(kwargs['hndfingerdist'])
self.__name = "barrett-bh8-282"
self.__hndnp = NodePath(self.__name)
self.__angle_open = self._compute_jawangle(self.__jawwidthopen)
self.__angle_close = self._compute_jawangle(self.__jawwidthclose)
print(self.__angle_open, self.__angle_close)
baselength = 79.5
fingerlength = 100
# eetippos/eetiprot
self.__eetip = np.array([
0.0, 0.0, baselength + fingerlength
]) + np.array([0.0, 0.0, self.__ftsensoroffset]) # max length 136
# base
# self.__hndbase.setrotmat(rm.rodrigues(np.array([0,0,1]), 180))
self.__hndbase.setpos(np.array([0.0, 0.0, self.__ftsensoroffset]))
# ftsensor
if self.__ftsensoroffset > 0:
self.__ftsensor = cm.CollisionModel(tp.Cylinder(
height=self.__ftsensoroffset, radius=30),
name="ftsensor")
self.__ftsensor.setPos(0, 0, -self.__ftsensoroffset / 2)
self.__ftsensor.reparentTo(self.__hndbase)
else:
self.__ftsensor = None
# 1,2 are the index and middle fingers, 3 is the thumb
self.__finger1_prox_pos = np.array([-25, 0, 41.5])
self.__finger1_prox_rot = rm.rotmat_from_euler(0, 0, -90)
self.__finger1_med_pos = np.array([50, 0, 33.9])
self.__finger1_med_rot = rm.rotmat_from_euler(90, 0, 0)
self.__finger1_dist_pos = np.array([69.94, 3, 0])
self.__finger1_dist_rot = rm.rotmat_from_euler(0, 0, -3)
self.__finger2_prox_pos = np.array([25, 0, 41.5])
self.__finger2_prox_rot = rm.rotmat_from_euler(0, 0, -90)
self.__finger2_med_pos = np.array([50, 0, 33.9])
self.__finger2_med_rot = rm.rotmat_from_euler(90, 0, 0)
self.__finger2_dist_pos = np.array([69.94, 3, 0])
self.__finger2_dist_rot = rm.rotmat_from_euler(0, 0, -3)
self.__finger3_med_pos = np.array([0, 50, 75.4])
self.__finger3_med_rot = rm.rotmat_from_euler(90, 0, 90)
self.__finger3_dist_pos = np.array([69.94, 3, 0])
self.__finger3_dist_rot = rm.rotmat_from_euler(0, 0, -3)
# controllable angles
self.__angle_main = 0.0
self.__angle_finger1 = 0.0
self.__angle_finger2 = 0.0
self.__angle_finger3 = 0.0
# angle ranges
self.__am_range = {"rngmin": 0, "rngmax": 180}
self.__af1_range = {"rngmin": 0, "rngmax": 140}
self.__af2_range = {"rngmin": 0, "rngmax": 140}
self.__af3_range = {"rngmin": 0, "rngmax": 140}
# update finger positions
# finger 1
finger1_prox_pos = self.__finger1_prox_pos
finger1_prox_rot = np.dot(
self.__finger1_prox_rot,
rm.rodrigues(np.array([0, 0, 1]), -self.__angle_main))
self.__finger1_prox.sethomomat(
rm.homobuild(finger1_prox_pos, finger1_prox_rot))
finger1_med_pos = finger1_prox_pos + np.dot(finger1_prox_rot,
self.__finger1_med_pos)
finger1_med_rot = np.dot(
finger1_prox_rot,
np.dot(self.__finger1_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger1)))
self.__finger1_med.sethomomat(
rm.homobuild(finger1_med_pos, finger1_med_rot))
finger1_dist_pos = finger1_med_pos + np.dot(finger1_med_rot,
self.__finger1_dist_pos)
finger1_dist_rot = np.dot(
finger1_med_rot,
np.dot(self.__finger1_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger1 / 3)))
self.__finger1_dist.sethomomat(
rm.homobuild(finger1_dist_pos, finger1_dist_rot))
self.__finger1_prox.reparentTo(self.__hndbase)
self.__finger1_med.reparentTo(self.__hndbase)
self.__finger1_dist.reparentTo(self.__hndbase)
# finger 2
finger2_prox_pos = self.__finger2_prox_pos
finger2_prox_rot = np.dot(
self.__finger2_prox_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_main))
self.__finger2_prox.sethomomat(
rm.homobuild(finger2_prox_pos, finger2_prox_rot))
finger2_med_pos = finger2_prox_pos + np.dot(finger2_prox_rot,
self.__finger2_med_pos)
finger2_med_rot = np.dot(
finger2_prox_rot,
np.dot(self.__finger2_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger2)))
self.__finger2_med.sethomomat(
rm.homobuild(finger2_med_pos, finger2_med_rot))
finger2_dist_pos = finger2_med_pos + np.dot(finger2_med_rot,
self.__finger2_dist_pos)
finger2_dist_rot = np.dot(
finger2_med_rot,
np.dot(self.__finger2_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger2 / 3)))
self.__finger2_dist.sethomomat(
rm.homobuild(finger2_dist_pos, finger2_dist_rot))
self.__finger2_prox.reparentTo(self.__hndbase)
self.__finger2_med.reparentTo(self.__hndbase)
self.__finger2_dist.reparentTo(self.__hndbase)
# finger 3
finger3_med_pos = self.__finger3_med_pos
finger3_med_rot = np.dot(
self.__finger3_med_rot,
rm.rodrigues(np.array([0, 0, 1]), self.__angle_finger3))
self.__finger3_med.sethomomat(
rm.homobuild(finger3_med_pos, finger3_med_rot))
finger3_dist_pos = finger3_med_pos + np.dot(finger3_med_rot,
self.__finger3_dist_pos)
finger3_dist_rot = np.dot(
finger3_med_rot,
np.dot(self.__finger3_dist_rot,
rm.rodrigues(np.array([0, 0, 1]),
self.__angle_finger3 / 3)))
self.__finger3_dist.sethomomat(
rm.homobuild(finger3_dist_pos, finger3_dist_rot))
#
self.__finger3_med.reparentTo(self.__hndbase)
self.__finger3_dist.reparentTo(self.__hndbase)
self.__hndbase.reparentTo(self.__hndnp)
# self.setjawwidth(self.__jawwidth)
self.setDefaultColor()
if self.__toggleframes:
if self.__ftsensor is not None:
self.__ftsensorframe = p3dh.genframe()
self.__ftsensorframe.reparentTo(self.__hndnp)
self.__hndframe = p3dh.genframe()
self.__hndframe.reparentTo(self.__hndnp)
self.__baseframe = p3dh.genframe()
self.__baseframe.reparentTo(self.__hndbase.objnp)
self.__finger1_prox_frame = p3dh.genframe()
self.__finger1_prox_frame.reparentTo(self.__finger1_prox.objnp)
self.__finger1_med_frame = p3dh.genframe()
self.__finger1_med_frame.reparentTo(self.__finger1_med.objnp)
self.__finger1_dist_frame = p3dh.genframe()
self.__finger1_dist_frame.reparentTo(self.__finger1_dist.objnp)
self.__finger2_prox_frame = p3dh.genframe()
self.__finger2_prox_frame.reparentTo(self.__finger2_prox.objnp)
self.__finger2_med_frame = p3dh.genframe()
self.__finger2_med_frame.reparentTo(self.__finger2_med.objnp)
self.__finger2_dist_frame = p3dh.genframe()
self.__finger2_dist_frame.reparentTo(self.__finger2_dist.objnp)
self.__finger3_med_frame = p3dh.genframe()
self.__finger3_med_frame.reparentTo(self.__finger3_med.objnp)
self.__finger3_dist_frame = p3dh.genframe()
self.__finger3_dist_frame.reparentTo(self.__finger3_dist.objnp)
# planecm.setMat(base.pg.np4ToMat4(planenode.gethomomat()))
# print(planenode.gethomomat())
# Boxes
# model = loader.loadModel('models/box.egg')
model = cm.CollisionModel("./objects/bunnysim.meshes")
node = bbd.BDTriangleBody(model, dynamic=True)
bulletnodelist = []
for i in range(3):
# node = bch.genBulletCDMesh(model)
# newnode = copy.deepcopy(node)
newnode = node.copy()
newnode.setMass(1)
rot = rm.rodrigues([0, 1, 0], -45)
pos = np.array([0, 0, 100 + i * 300])
newnode.set_homomat(rm.homobuild(pos, rot))
print(newnode.get_homomat())
newnode.set_homomat(rm.homobuild(pos, rot))
print(newnode.get_homomat())
base.physicsworld.attachRigidBody(newnode)
bulletnodelist.append(newnode)
# modelcopy = copy.deepcopy(model)
# modelcopy.setColor(.8, .6, .3, .5)
# modelcopy.reparentTo(np)
# debugNode = BulletDebugNode('Debug')
# debugNode.showWireframe(True)
# debugNode.showConstraints(True)
# debugNode.showBoundingBoxes(False)
# debugNode.showNormals(False)
# debugNP = base.render.attachNewNode(debugNode)
def rgtcapture(self):
"""
capture one goal pose using the rgt hndcam system
:return: a 4x4 h**o numpy mat
author: weiwei
date: 20180926
"""
# goalT = self.hc.getObjPoseRgtCam(self.rgtwatchpos, self.rgtwatchrotmat, marker_id=self.markerid)
posG = base.pg.npToMat4(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
np.array([600, 0, 973 - 15]))
# print(posG)
# relpos1 = np.array([-60.14147593, -0.43612779, 40.08426847])
# relrot1 = np.array([[0.9672951, 0.003912, 0.25362363],
# [-0.00265115, 0.99998247, -0.00531303],
# [-0.25363992, 0.00446691, 0.9672884]])
relpos0 = np.array([-65, 0, 65])
relrot0 = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
rel0 = base.pg.npToMat4(relrot0, relpos0)
posL0 = rel0 * posG
virtualgoalpos0 = np.array([posL0[3][0], posL0[3][1], posL0[3][2]])
virtualgoalrot0 = np.array([[posL0[0][0], posL0[1][0], posL0[2][0]],
[posL0[0][1], posL0[1][1], posL0[2][1]],
[posL0[0][2], posL0[1][2], posL0[2][2]]])
relpos1 = np.array([-65, 0, 50])
relrot1 = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
rel1 = base.pg.npToMat4(relrot1, relpos1)
posL1 = rel1 * posG
virtualgoalpos1 = np.array([posL1[3][0], posL1[3][1], posL1[3][2]])
virtualgoalrot1 = np.array([[posL1[0][0], posL1[1][0], posL1[2][0]],
[posL1[0][1], posL1[1][1], posL1[2][1]],
[posL1[0][2], posL1[1][2], posL1[2][2]]])
# virtualgoalpos0 = np.array([480,0,973-76+75+20])
# virtualgoalrot0 = rm.rodrigues([0,1,0,], 20)
# virtualgoalpos1 = np.array([450,250,1200])
# virtualgoalrot1 = rm.rodrigues([0,1,0,], -90)
virtualgoalpos2 = np.array([490,250,1200])
virtualgoalrot2 = rm.rodrigues([0,1,0,], -90)
virtualgoalpos3 = np.array([490,350,1200])
virtualgoalrot3 = rm.rodrigues([0,1,0,], -90)
goalTlist = [rm.homobuild(virtualgoalpos0, virtualgoalrot0),
rm.homobuild(virtualgoalpos1, virtualgoalrot1),
rm.homobuild(virtualgoalpos2, virtualgoalrot2),
rm.homobuild(virtualgoalpos3, virtualgoalrot3)]
ngoal = len(self.goallist)
if ngoal >= len(goalTlist):
return False
goalT = goalTlist[ngoal]
if goalT is None:
return False
else:
self.goallist.append(goalT)
tmpobjcm = copy.deepcopy(self.objcm)
tmpobjcm.setMat(base.pg.np4ToMat4(goalT))
tmpobjcm.reparentTo(base.render)
tmpobjcm.showLocalFrame()
self.objrenderlist.append(tmpobjcm)
for id, tmpobjcm in enumerate(self.objrenderlist):
tmpobjcm.setColor(0,(id)/float(len(self.objrenderlist)),(id+1)/float(len(self.objrenderlist)), 0.9)
return True
