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 __updatefk(self, armlj):
"""
Update the structure of hrp5's arm links and joints (single)
Note that this function should not be called explicitly
It is called automatically by functions like movexxx
:param armlj: the rgtlj or lftlj robot structure
:return: null
author: weiwei
date: 20161202
"""
i = 1
while i != -1:
j = armlj[i]['mother']
armlj[i]['linkpos'] = armlj[j]['linkend']
if i != 3 and i != 5:
armlj[i]['rotmat'] = np.dot(
np.dot(armlj[j]['rotmat'], armlj[i]['inherentR']),
rm.rodrigues(armlj[i]['rotax'], armlj[i]['rotangle']))
else:
armlj[i]['rotmat'] = np.dot(
armlj[j]['rotmat'],
rm.rodrigues(armlj[i]['rotax'], armlj[i]['rotangle']))
armlj[i]['linkend'] = np.squeeze(
np.dot(armlj[i]['rotmat'], armlj[i]['linkvec'].reshape(
(-1, 1))).reshape((1, -1))) + armlj[i]['linkpos']
i = armlj[i]['child']
return armlj
def movewaist(self, rotangle=0):
"""
rotate the base of the robot
:param rotangle: in degree
:return: null
author: weiwei
date: 20161107
"""
# right arm
self.rgtarm[0]['rotangle'] = rotangle
self.rgtarm[0]['rotmat'] = np.dot(
self.rgtarm[0]['refcs'],
rm.rodrigues(self.rgtarm[0]['rotax'], self.rgtarm[0]['rotangle']))
self.rgtarm[0]['linkend'] = np.dot(
self.rgtarm[0]['rotmat'],
self.rgtarm[0]['linkvec']) + self.rgtarm[0]['linkpos']
self.__updatefk(self.rgtarm)
# left arm
self.lftarm[0]['rotangle'] = rotangle
self.lftarm[0]['rotmat'] = np.dot(
self.lftarm[0]['refcs'],
rm.rodrigues(self.lftarm[0]['rotax'], self.lftarm[0]['rotangle']))
self.lftarm[0]['linkend'] = np.dot(
self.lftarm[0]['rotmat'],
self.lftarm[0]['linkvec']) + self.lftarm[0]['linkpos']
self.__updatefk(self.lftarm)
def rot_transform(rot_mat, incline_angle):
"""
Set the inclination angle of the object according
:param rot_mat: initial rotation matrix of the robot arm
:param incline_angle: inclination angle about the end effector Z axis
:return: rotation matrix of the required inclination
"""
tmp_rot1 = rm.rodrigues(rot_mat[:, 2], 90)
tmp_rot1 = np.dot(tmp_rot1, rot_mat)
tmp_rot2 = rm.rodrigues(tmp_rot1[:, 2], incline_angle)
return np.dot(tmp_rot2, tmp_rot1)
def __init__(self, betransparent=False):
"""
load obstacles model
separated by category
:param base:
author: weiwei
date: 20181205
"""
self.__this_dir, _ = os.path.split(__file__)
# table
self.__tablepath = os.path.join(self.__this_dir, "obstacles", "yumi_tablenotop.stl")
self.__tablecm = cm.CollisionModel(self.__tablepath, betransparent)
self.__tablecm.setColor(.55, .55, .5, 1.0)
self.__tablecm.setColor(.45, .45, .35, 1.0)
# housing
## housing pillar
## TODO these models could be replaced by trimesh.primitives
self.__beam2100path = os.path.join(self.__this_dir, "obstacles", "yumi_column60602100.stl")
self.__beam540path = os.path.join(self.__this_dir, "obstacles", "yumi_column6060540.stl")
self.__pillarrgtcm = cm.CollisionModel(self.__beam2100path)
self.__pillarrgtcm.setColor(.5, .5, .55, 1.0)
self.__pillarrgtcm.setPos(-327.0, -240.0, -1015.0)
self.__pillarlftcm = copy.deepcopy(self.__pillarrgtcm)
self.__pillarlftcm.setColor(.5, .5, .55, 1.0)
self.__pillarlftcm.setPos(-327.0, 240.0, -1015.0)
## housing rgt
self.__rowbackcm = cm.CollisionModel(self.__beam540path)
self.__rowbackcm.setColor(.5, .5, .55, 1.0)
self.__rowbackcm.setPos(-327.0, 0.0, 1085.0)
self.__rowrgtcm = copy.deepcopy(self.__rowbackcm)
self.__rowrgtcm.setColor(.5, .5, .55, 1.0)
homomat = self.__rowrgtcm.gethomomat()
homomat[:3,:3] = rm.rodrigues([0,0,1],90)
self.__rowrgtcm.sethomomat(homomat)
self.__rowrgtcm.setPos(-27.0, -240.0, 1085.0)
self.__rowlftcm = copy.deepcopy(self.__rowbackcm)
self.__rowlftcm.setColor(.5, .5, .55, 1.0)
homomat = self.__rowlftcm.gethomomat()
homomat[:3,:3] = rm.rodrigues([0,0,1],90)
self.__rowlftcm.sethomomat(homomat)
self.__rowlftcm.setPos(-27.0, 240.0, 1085.0)
self.__rowfrontcm = copy.deepcopy(self.__rowbackcm)
self.__rowfrontcm.setColor(.5, .5, .55, 1.0)
self.__rowfrontcm.setPos(273.0, -0.0, 1085.0)
self.__battached = False
self.__changableobslist = []
def define_grasp_with_rotation(hndfa, grasp_center, finger_normal, hand_normal, jawwidth, objcm,
rotation_interval=15, rotation_range=(-90, 90), toggleflip = True):
"""
:param hndfa:
:param grasp_center:
:param finger_normal:
:param hand_normal:
:param jawwidth:
:param objcm:
:param rotation_interval:
:param rotation_range:
:param toggleflip:
:return:
author: chenhao, revised by weiwei
date: 20200104
"""
effect_grasp = []
for rotate_angle in range(rotation_range[0], rotation_range[1], rotation_interval):
hnd = hndfa.genHand()
hand_normal_rotated = np.dot(rm.rodrigues(finger_normal, rotate_angle), np.asarray(hand_normal))
grasp = hnd.approachat(grasp_center[0], grasp_center[1], grasp_center[2],
finger_normal[0], finger_normal[1], finger_normal[2],
hand_normal_rotated[0], hand_normal_rotated[1], hand_normal_rotated[2], jawwidth=jawwidth)
# if not ishndobjcollided(hndfa, grasp[0], grasp[2], objcm) == False:
effect_grasp.append(grasp)
if toggleflip:
grasp_flipped = hnd.approachat(grasp_center[0], grasp_center[1], grasp_center[2],
-finger_normal[0], -finger_normal[1], -finger_normal[2],
hand_normal_rotated[0], hand_normal_rotated[1], hand_normal_rotated[2], jawwidth=jawwidth)
if not ishndobjcollided(hndfa, grasp_flipped[0], grasp_flipped[2], objcm):
effect_grasp.append(grasp_flipped)
return effect_grasp
def __updatefk(self, armlj):
"""
Update the structure of hiro's arm links and joints (single)
Note that this function should not be called explicitly
It is called automatically by functions like movexxx
:param armlj: the rgtlj or lftlj robot structure
:return: null
author: weiwei
date: 20160615
"""
i = 1
while i != -1:
j = armlj[i]['mother']
armlj[i]['linkpos'] = armlj[j]['linkend']
armlj[i]['refcs'] = np.dot(armlj[j]['rotmat'],
armlj[i]['inherentR'])
armlj[i]['rotmat'] = np.dot(
armlj[i]['refcs'],
rm.rodrigues(armlj[i]['rotax'], armlj[i]['rotangle']))
armlj[i]['linkend'] = np.dot(
armlj[i]['rotmat'], armlj[i]['linkvec']) + armlj[i]['linkpos']
i = armlj[i]['child']
return armlj
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 gripat(self,
fcx,
fcy,
fcz,
c0nx,
c0ny,
c0nz,
rotangle=0,
jawwidth=None):
"""
set the hand to grip at fcx, fcy, fcz, fc = finger center
the normal of the sglfgr contact is set to be c0nx, c0ny, c0nz
the rotation around the normal is set to rotangle
the jaw_width is set to jaw_width
date: 20170322
author: weiwei
"""
# x is the opening direction of the hand, _org means the value when rotangle=0
standardx_org = np.array([1, 0, 0])
newx_org = np.array([c0nx, c0ny, c0nz])
rotangle_org = rm.degree_betweenvector(newx_org, standardx_org)
rotaxis_org = np.array([0, 0, 1])
if not (np.isclose(rotangle_org, 180.0)
or np.isclose(rotangle_org, 0.0)):
rotaxis_org = rm.unit_vector(np.cross(standardx_org, newx_org))
newrotmat_org = rm.rodrigues(rotaxis_org, rotangle_org)
# rotate to the given rotangle
hnd_rotmat4 = np.eye(4)
hnd_rotmat4[:3, :3] = np.dot(rm.rodrigues(newx_org, rotangle),
newrotmat_org)
handtipnpvec3 = np.array([fcx, fcy, fcz]) - np.dot(
hnd_rotmat4[:3, :3], self.__eetip)
hnd_rotmat4[:3, 3] = handtipnpvec3
self.__hndnp.setMat(p3dh.npmat4_to_pdmat4(hnd_rotmat4))
if jawwidth is None:
jawwidth = self.__jawwidthopen
self.setjawwidth(jawwidth)
return [jawwidth, np.array([fcx, fcy, fcz]), hnd_rotmat4]
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 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
objgpos = np.array([304.5617667638, -277.1026725769, 1101.0])
objgrot = np.array([[1.0, 0.0, 0.0], [0.0, 6.12323426293e-17, -1.0],
[0.0, 1.0, 6.12323426293e-17]]).T
objcmcopys = copy.deepcopy(objcm)
objcmcopys.setColor(0.0, 0.0, 1.0, .3)
objcmcopys.setMat(base.pg.npToMat4(npmat3=objstrot, npvec3=objstpos))
objcmcopys.reparentTo(base.render)
startpos = np.array([354.5617667638, -256.0889005661, 1060.5])
startrot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]).T
goalpos3 = np.array([354.5617667638, -256.0889005661, 1150.5])
goalrot3 = np.array([[1, 0, 0], [0, -0.92388, -0.382683],
[0, 0.382683, -0.92388]]).T
goalpos3 = goalpos3 - 70.0 * goalrot3[:, 2]
goalpos4 = np.array([454.5617667638, -100, 1150.5])
goalrot4 = np.dot(rm.rodrigues([0, 0, 1], -120), goalrot3)
goalpos4 = goalpos4 - 250.0 * goalrot4[:, 2]
# goalpos2 = goalpos2 - 150.0*goalrot2[:,2]
start = robot.numik(startpos, startrot, armname)
goal3 = robot.numikmsc(goalpos3, goalrot3, msc=start, armname=armname)
print(goal3)
goal4 = robot.numikmsc(goalpos4, goalrot4, msc=start, armname=armname)
print(goal4)
planner = rrtc.RRTConnect(start=goal3,
goal=goal4,
ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=5,
maxiter=2000,
maxtime=100.0)
def findtubes(self, tubestand_homomat, tgtpcdnp, toggledebug=False):
"""
:param tubestand_homomat:
:param tgtpcdnp:
:return:
"""
elearray = np.zeros((5, 10))
eleconfidencearray = np.zeros((5, 10))
tgtpcdo3d = o3dh.nparray_to_o3dpcd(tgtpcdnp)
tgtpcdo3d_removed = o3dh.remove_outlier(tgtpcdo3d,
downsampling_voxelsize=None,
nb_points=90,
radius=5)
tgtpcdnp = o3dh.o3dpcd_to_parray(tgtpcdo3d_removed)
# transform tgtpcdnp back
tgtpcdnp_normalized = rm.homotransformpointarray(
rm.homoinverse(tubestand_homomat), tgtpcdnp)
if toggledebug:
cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render)
tscm2 = cm.CollisionModel("../objects/tubestand.stl")
tscm2.reparentTo(base.render)
for i in range(5):
for j in range(10):
holepos = self.tubeholecenters[i][j]
# squeeze the hole size by half
tmppcd = tgtpcdnp_normalized[
tgtpcdnp_normalized[:, 0] < holepos[0] +
self.tubeholesize[0] / 1.9]
tmppcd = tmppcd[tmppcd[:, 0] > holepos[0] -
self.tubeholesize[0] / 1.9]
tmppcd = tmppcd[tmppcd[:, 1] < holepos[1] +
self.tubeholesize[1] / 1.9]
tmppcd = tmppcd[tmppcd[:, 1] > holepos[1] -
self.tubeholesize[1] / 1.9]
tmppcd = tmppcd[tmppcd[:, 2] > 70]
if len(tmppcd) > 100:
print(
"------more than 100 raw points, start a new test------"
)
# use core tmppcd to decide tube types (avoid noises)
coretmppcd = tmppcd[tmppcd[:, 0] < holepos[0] +
self.tubeholesize[0] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 0] > holepos[0] -
self.tubeholesize[0] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 1] < holepos[1] +
self.tubeholesize[1] / 4]
coretmppcd = coretmppcd[coretmppcd[:, 1] > holepos[1] -
self.tubeholesize[1] / 4]
print("testing the number of core points...")
print(len(coretmppcd[:, 2]))
if len(coretmppcd[:, 2]) < 10:
print("------the new test is done------")
continue
if np.max(tmppcd[:, 2]) > 100:
candidatetype = 1
tmppcd = tmppcd[tmppcd[:, 2] >
100] # crop tmppcd for better charge
else:
candidatetype = 2
tmppcd = tmppcd[tmppcd[:, 2] < 90]
if len(tmppcd) < 10:
continue
print("passed the core points test, rotate around...")
rejflag = False
for angle in np.linspace(0, 180, 20):
tmphomomat = np.eye(4)
tmphomomat[:3, :3] = rm.rodrigues(
tubestand_homomat[:3, 2], angle)
newtmppcd = rm.homotransformpointarray(
tmphomomat, tmppcd)
minstd = np.min(np.std(newtmppcd[:, :2], axis=0))
print(minstd)
if minstd < 1.3:
rejflag = True
print("rotate round done")
if rejflag:
continue
else:
tmpangles = np.arctan2(tmppcd[:, 1], tmppcd[:, 0])
tmpangles[
tmpangles < 0] = 360 + tmpangles[tmpangles < 0]
print(np.std(tmpangles))
print("ACCEPTED! ID: ", i, j)
elearray[i][j] = candidatetype
eleconfidencearray[i][j] = 1
if toggledebug:
# normalized
objnp = p3dh.genpointcloudnodepath(tmppcd, pntsize=5)
rgb = np.random.rand(3)
objnp.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp.reparentTo(base.render)
stick = p3dh.gendumbbell(
spos=np.array([holepos[0], holepos[1], 10]),
epos=np.array([holepos[0], holepos[1], 60]))
stick.setColor(rgb[0], rgb[1], rgb[2], 1)
stick.reparentTo(base.render)
# original
tmppcd_tr = rm.homotransformpointarray(
tubestand_homomat, tmppcd)
objnp_tr = p3dh.genpointcloudnodepath(tmppcd_tr,
pntsize=5)
objnp_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp_tr.reparentTo(base.render)
spos_tr = rm.homotransformpoint(
tubestand_homomat,
np.array([holepos[0], holepos[1], 0]))
stick_tr = p3dh.gendumbbell(
spos=np.array([spos_tr[0], spos_tr[1], 10]),
epos=np.array([spos_tr[0], spos_tr[1], 60]))
stick_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
stick_tr.reparentTo(base.render)
# box normalized
center, bounds = rm.get_aabb(tmppcd)
boxextent = np.array([
bounds[0, 1] - bounds[0, 0],
bounds[1, 1] - bounds[1, 0],
bounds[2, 1] - bounds[2, 0]
])
boxhomomat = np.eye(4)
boxhomomat[:3, 3] = center
box = p3dh.genbox(extent=boxextent,
homomat=boxhomomat,
rgba=np.array(
[rgb[0], rgb[1], rgb[2], .3]))
box.reparentTo(base.render)
# box original
center_r = rm.homotransformpoint(
tubestand_homomat, center)
boxhomomat_tr = copy.deepcopy(tubestand_homomat)
boxhomomat_tr[:3, 3] = center_r
box_tr = p3dh.genbox(extent=boxextent,
homomat=boxhomomat_tr,
rgba=np.array(
[rgb[0], rgb[1], rgb[2], .3]))
box_tr.reparentTo(base.render)
print("------the new test is done------")
return elearray, eleconfidencearray
from direct.filter.CommonFilters import CommonFilters
import manipulation.grip.robotiq85.rtq85 as rtq85
from robotsim.ur3dual import ur3dual
from robotsim.ur3dual import ur3dualmesh
import utiltools.robotmath as rm
# loadPrcFileData("", "want-directtools #t")
# loadPrcFileData("", "want-tk #t")
base = pandactrl.World(camp=[3000, 3000, 3000], lookatp=[0, 0, 1300])
rgthnd = rtq85.Rtq85()
lfthnd = rtq85.Rtq85()
ur3dualrobot = ur3dual.Ur3DualRobot(position=[0, 500, 0],
rotmat=rm.rodrigues([0, 0, 1], 70))
# goalpos = np.array([200,120,1410])
# goalrot = np.array([[1,0,0],[0,0,-1],[0,1,0]])
# goal = ur3dualrobot.numik(goalpos, goalrot, armname = 'lft')
# goal = np.array([-46.251876352032305, -41.418654079396184, 94.34173209615693, -15.917387039376576, 92.07172082393664, -25.134340575525133])
goal = np.array([
-209.13573775, -42.61307312, -283.86285256, 30.51538756, -231.85631837,
-218.24860474
])
ur3dualrobot.movearmfk(goal, armname='rgt')
# ur3dualrobot.gozeropose()
ur3dualrobotball = Ur3DualBall()
# bcndict = ur3dualrobotball.genfullbcndict(ur3dualrobot)
# bcndict = ur3dualrobotball.gensemibcndict(ur3dualrobot)
# bcndict = ur3dualrobotball.genholdbcndict(ur3dualrobot, armname='rgt')
# bcndict = ur3dualrobotball.genfullactivebcndict(ur3dualrobot)
def plotPortionCircArrow(self,
nodepath,
axis=None,
torqueportion=0.0,
center=None,
radius=5.0,
thickness=1.5,
rgba=None,
discretization=24,
plotname="circarrow"):
"""
Draw a circule arrow on a solid circle
Especially used for visualization of torque
the arraw starts from (axis cross [1,0,0])*radius+center
if axis cross [1,0,0] equals [0,0,0], the arrow starts from (axis cross [0,1,0])*radius+center
the portion of the circ arrow indicates the magnitude of t, maxt is set to torque/25.0
if torque/25.0 is larger than 1.0, set it to 1.0
:param nodepath:
:param torque portion: measured torque over maximum value 0.0~1.0
:param center: center of the circle
:param radius: radius of the circle
:param rgba: color of the arrow
:param discretization: number sticks used
:return:
author: weiwei
date: 20180227
"""
if axis is None:
axis = np.array([0.0, 0.0, 1.0])
if center is None:
center = np.array([0.0, 0.0, 0.0])
else:
center = np.array([center[0], center[1], center[2]])
if rgba is None:
rgba = np.array([1.0, 1.0, 1.0, 1.0])
xaxis = np.array([1.0, 0.0, 0.0])
yaxis = np.array([0.0, 1.0, 0.0])
startingaxis = np.cross(axis, xaxis)
if np.linalg.norm(startingaxis) < .1:
startingaxis = np.cross(axis, yaxis)
startingaxis = startingaxis / np.linalg.norm(startingaxis)
startingpos = startingaxis * radius + center
cirarrnp = NodePath(plotname)
discretizedangle = 360.0 / discretization
ndist = int(torqueportion * discretization)
lastpos = startingpos
if abs(ndist) > 1:
for i in range(1 * np.sign(ndist), ndist, 1 * np.sign(ndist)):
nxtpos = center + np.dot(
rm.rodrigues(axis, i * discretizedangle),
startingaxis.T).T * radius
self.plotStick(cirarrnp,
spos=lastpos,
epos=nxtpos,
thickness=thickness,
rgba=rgba,
plotname="circarrseg")
lastpos = nxtpos
nxtposend = center + np.dot(
rm.rodrigues(axis, ndist * discretizedangle),
startingaxis.T).T * radius
self.plotArrow(cirarrnp,
lastpos,
nxtposend,
length=1.0,
thickness=thickness,
rgba=rgba)
cirarrnpbg = NodePath(plotname + "bg")
discretizedangle = 360.0 / discretization
lastpos = startingpos
for i in range(1, discretization + 1):
nxtpos = center + np.dot(rm.rodrigues(axis, i * discretizedangle),
startingaxis.T).T * radius
bgrgba = np.array([rgba[0], rgba[1], rgba[2], .1])
self.plotStick(cirarrnp,
spos=lastpos,
epos=nxtpos,
thickness=thickness * .9,
rgba=bgrgba,
plotname="circarrseg")
lastpos = nxtpos
cirarrnpbg.reparentTo(cirarrnp)
cirarrnp.reparentTo(nodepath)
return cirarrnp
if __name__ == "__main__":
import environment.pandacdhelper as cmcd
import os
import numpy as np
import utiltools.robotmath as rm
import pandaplotutils.pandactrl as pc
base = pc.World(camp=[1000, 300, 1000], lookatp=[0, 0, 0])
this_dir, this_filename = os.path.split(__file__)
objpath = os.path.join(this_dir, "objects", "bunnysim.stl")
bunnycm = CollisionModel(objpath, type="box")
bunnycm.setColor(0.7, 0.7, 0.0, 1.0)
bunnycm.reparentTo(base.render)
rotmat = rm.rodrigues([1, 0, 0], 90)
bunnycm.setMat(base.pg.npToMat4(rotmat))
bunnycm1 = CollisionModel(objpath, type="box")
bunnycm1.setColor(0.7, 0, 0.7, 1.0)
bunnycm1.reparentTo(base.render)
# rotmat = rm.rodrigues([0,0,1], 15)
rotmat = rm.euler_matrix(0, 0, 15)
bunnycm1.setMat(base.pg.npToMat4(rotmat, np.array([0, 10, 0])))
bunnycm2 = CollisionModel(objpath, type="box")
bunnycm2.setColor(0, 0.7, 0.7, 1.0)
bunnycm2.reparentTo(base.render)
rotmat = rm.rodrigues([1, 0, 0], -45)
bunnycm2.setMat(base.pg.npToMat4(rotmat, np.array([0, 200, 0])))
def __initlftlj(self):
"""
Init hrp5's lft arm links and joints
## note
x is facing forward, y is facing left, z is facing upward
each element of lftlj is a dictionary
lftlj[i]['linkpos'] indicates the position of a link
lftlj[i]['linkvec'] indicates the vector of a link that points from start to end
lftlj[i]['rotmat'] indicates the frame of this link
lftlj[i]['rotax'] indicates the rotation axis of the link
lftlj[i]['rotangle'] indicates the rotation angle of the link around the rotax
lftlj[i]['linkend'] indicates the end position of the link (passively computed)
## more note:
lftlj[1]['linkpos'] is the position of the first joint
lftlj[i]['linkend'] is the same as lftlj[i+1]['linkpos'],
I am keeping this value for the eef (end-effector)
## even more note:
joint is attached to the linkpos of a link
for the first link, the joint is fixed and the rotax = 0,0,0
## inherentR is only available at the first link
:return:
lftlj:
a list of dictionaries with each dictionary holding name, mother, child,
linkpos (link position), linkvec (link vector), rotmat (orientation), rotax (rotation axis of a joint),
rotangle (rotation angle of the joint around rotax)
linkend (the end position of a link) is computed using rotmat, rotax, linkpos, and linklen
lj means link and joint, the joint attached to the link is at the linkend
author: weiwei
date: 20161202, tsukuba
"""
# create a arm with 6 joints
lftlj = [dict() for i in range(7)]
rngsafemargin = 0
# the 0th link and joint
lftlj[0]['name'] = 'link0'
lftlj[0]['mother'] = -1
lftlj[0]['child'] = 1
lftlj[0]['linkpos'] = np.array([0, 0, 0])
lftlj[0]['linkvec'] = np.array([0, 237.50, 954.39]) + np.dot(
rm.rodrigues([1, 0, 0], -135), np.array([0, 0, 89.159]))
lftlj[0]['rotax'] = np.array([0, 0, 1])
lftlj[0]['rotangle'] = 0
lftlj[0]['rotmat'] = np.eye(3)
lftlj[0]['linkend'] = np.dot(lftlj[0]['rotmat'],
lftlj[0]['linkvec']) + lftlj[0]['linkpos']
# the 1st joint and link
lftlj[1]['name'] = 'link1'
lftlj[1]['mother'] = 0
lftlj[1]['child'] = 2
lftlj[1]['linkpos'] = lftlj[0]['linkend']
lftlj[1]['linkvec'] = np.array([0, 135.85, 0])
lftlj[1]['rotax'] = np.array([0, 0, 1])
lftlj[1]['rotangle'] = 0
lftlj[1]['inherentR'] = np.dot(rm.rodrigues([0, 0, 1], 180),
rm.rodrigues([1, 0, 0], 135))
lftlj[1]['rotmat'] = np.dot(np.dot(lftlj[0]['rotmat'], lftlj[1]['inherentR']), \
rm.rodrigues(lftlj[1]['rotax'], lftlj[1]['rotangle']))
lftlj[1]['linkend'] = np.dot(lftlj[1]['rotmat'],
lftlj[1]['linkvec']) + lftlj[1]['linkpos']
lftlj[1]['rngmin'] = -(360 - rngsafemargin)
lftlj[1]['rngmax'] = +(360 - rngsafemargin)
# the 2nd joint and link
lftlj[2]['name'] = 'link2'
lftlj[2]['mother'] = 1
lftlj[2]['child'] = 3
lftlj[2]['linkpos'] = lftlj[1]['linkend']
lftlj[2]['linkvec'] = np.array([0, -119.70, 425.00])
lftlj[2]['rotax'] = np.array([0, 1, 0])
lftlj[2]['rotangle'] = 0
lftlj[2]['inherentR'] = rm.rodrigues([0, 1, 0], 90)
lftlj[2]['rotmat'] = np.dot(np.dot(lftlj[1]['rotmat'], lftlj[2]['inherentR']), \
rm.rodrigues(lftlj[2]['rotax'], lftlj[2]['rotangle']))
lftlj[2]['linkend'] = np.dot(lftlj[2]['rotmat'],
lftlj[2]['linkvec']) + lftlj[2]['linkpos']
lftlj[2]['rngmin'] = -(360 - rngsafemargin)
lftlj[2]['rngmax'] = +(360 - rngsafemargin)
# the 3rd joint and link
lftlj[3]['name'] = 'link3'
lftlj[3]['mother'] = 2
lftlj[3]['child'] = 4
lftlj[3]['linkpos'] = lftlj[2]['linkend']
lftlj[3]['linkvec'] = np.array([0, 0, 392.25])
lftlj[3]['rotax'] = np.array([0, 1, 0])
lftlj[3]['rotangle'] = 0
lftlj[3]['rotmat'] = np.dot(
lftlj[2]['rotmat'],
rm.rodrigues(lftlj[3]['rotax'], lftlj[3]['rotangle']))
lftlj[3]['linkend'] = np.dot(lftlj[3]['rotmat'],
lftlj[3]['linkvec']) + lftlj[3]['linkpos']
lftlj[3]['rngmin'] = -(360 - rngsafemargin)
lftlj[3]['rngmax'] = +(360 - rngsafemargin)
# the 4th joint and link
lftlj[4]['name'] = 'link4'
lftlj[4]['mother'] = 3
lftlj[4]['child'] = 5
lftlj[4]['linkpos'] = lftlj[3]['linkend']
lftlj[4]['linkvec'] = np.array([0, 93.00, 0])
lftlj[4]['rotax'] = np.array([0, 1, 0])
lftlj[4]['rotangle'] = 0
lftlj[4]['inherentR'] = rm.rodrigues([0, 1, 0], 90)
lftlj[4]['rotmat'] = np.dot(np.dot(lftlj[3]['rotmat'], lftlj[4]['inherentR']), \
rm.rodrigues(lftlj[4]['rotax'], lftlj[4]['rotangle']))
lftlj[4]['linkend'] = np.dot(lftlj[4]['rotmat'],
lftlj[4]['linkvec']) + lftlj[4]['linkpos']
lftlj[4]['rngmin'] = -(360 - rngsafemargin)
lftlj[4]['rngmax'] = +(360 - rngsafemargin)
# the 5th joint and link
lftlj[5]['name'] = 'link5'
lftlj[5]['mother'] = 4
lftlj[5]['child'] = 6
lftlj[5]['linkpos'] = lftlj[4]['linkend']
lftlj[5]['linkvec'] = np.array([0, 82.30, 94.65])
lftlj[5]['rotax'] = np.array([0, 0, 1])
lftlj[5]['rotangle'] = 0
lftlj[5]['rotmat'] = np.dot(
lftlj[4]['rotmat'],
rm.rodrigues(lftlj[5]['rotax'], lftlj[5]['rotangle']))
lftlj[5]['linkend'] = np.dot(lftlj[5]['rotmat'],
lftlj[5]['linkvec']) + lftlj[5]['linkpos']
lftlj[5]['rngmin'] = -(360 - rngsafemargin)
lftlj[5]['rngmax'] = +(360 - rngsafemargin)
# the 6th joint and link
lftlj[6]['name'] = 'link6'
lftlj[6]['mother'] = 5
lftlj[6]['child'] = -1
lftlj[6]['linkpos'] = lftlj[5]['linkend']
# for hrp5three
# lftlj[6]['linkvec'] = np.array([-172.7,0,0])
# for rtq85
lftlj[6]['linkvec'] = np.array([-13.5 - 145.0, 0, 0])
lftlj[6]['rotax'] = np.array([1, 0, 0])
lftlj[6]['rotangle'] = 0
lftlj[6]['inherentR'] = rm.rodrigues([0, 0, 1], -90)
lftlj[6]['rotmat'] = np.dot(np.dot(lftlj[5]['rotmat'], lftlj[6]['inherentR']), \
rm.rodrigues(lftlj[6]['rotax'], lftlj[6]['rotangle']))
lftlj[6]['linkend'] = np.dot(lftlj[6]['rotmat'],
lftlj[6]['linkvec']) + lftlj[6]['linkpos']
lftlj[6]['rngmin'] = -(360 - rngsafemargin)
lftlj[6]['rngmax'] = +(360 - rngsafemargin)
return lftlj
def __initlftarmj(self):
"""
Init hrp5's lft arm links and joints
## note
x is facing forward, y is facing left, z is facing upward
each element of lftlj is a dictionary
lftlj[i]['linkpos'] indicates the position of a link
lftlj[i]['linkvec'] indicates the vector of a link that points from start to end
lftlj[i]['rotmat'] indicates the frame of this link
lftlj[i]['rotax'] indicates the rotation axis of the link
lftlj[i]['rotangle'] indicates the rotation angle of the link around the rotax
lftlj[i]['linkend'] indicates the end position of the link (passively computed)
lftlj[i]['inherentR'] is the inherent rotation of the joint
lftlj[i]['refcs'] is the reference coordinate system of the joint
## more note:
lftlj[1]['linkpos'] is the position of the first joint
lftlj[i]['linkend'] is the same as lftlj[i+1]['linkpos'],
I am keeping this value for the eef (end-effector)
## even more note:
joint is attached to the linkpos of a link
for the first link, the joint is fixed and the rotax = 0,0,0
:return:
lftlj:
a list of dictionaries with each dictionary holding name, mother, child,
linkpos (link position), linkvec (link vector), rotmat (orientation), rotax (rotation axis of a joint),
rotangle (rotation angle of the joint around rotax)
linkend (the end position of a link) is computed using rotmat, rotax, linkpos, and linklen
lj means link and joint, the joint attached to the link is at the linkend
author: weiwei
date: 20161202, tsukuba, 20190328, toyonaka
"""
# create a arm with 6 joints
lftlj = [dict() for i in range(7)]
rngsafemargin = 0
# the 0th link and joint
lftlj[0]['name'] = 'link0'
lftlj[0]['mother'] = -1
lftlj[0]['child'] = 1
lftlj[0]['linkpos'] = self.__pos
lftlj[0]['linkvec'] = np.array([0, 258.485281374, 1610.51471863])+\
np.dot(rm.rodrigues([1,0,0], -135), np.array([0,0,0])) #89.159
lftlj[0]['rotax'] = np.array([0, 0, 1])
lftlj[0]['rotangle'] = 0
lftlj[0]['inherentR'] = self.__rotmat
lftlj[0]['refcs'] = lftlj[0]['inherentR']
lftlj[0]['rotmat'] = np.dot(
lftlj[0]['refcs'],
rm.rodrigues(lftlj[0]['rotax'], lftlj[0]['rotangle']))
lftlj[0]['linkend'] = np.dot(lftlj[0]['rotmat'],
lftlj[0]['linkvec']) + lftlj[0]['linkpos']
lftlj[0]['rngmin'] = -(0 - rngsafemargin)
lftlj[0]['rngmax'] = +(0 - rngsafemargin)
# the 1st joint and link
lftlj[1]['name'] = 'link1'
lftlj[1]['mother'] = 0
lftlj[1]['child'] = 2
lftlj[1]['linkpos'] = lftlj[0]['linkend']
lftlj[1]['linkvec'] = np.array([0, -119.8, 151.9])
lftlj[1]['rotax'] = np.array([0, 0, 1])
lftlj[1]['rotangle'] = 0
lftlj[1]['inherentR'] = rm.rodrigues([1, 0, 0], -135)
lftlj[1]['refcs'] = np.dot(lftlj[0]['rotmat'], lftlj[1]['inherentR'])
lftlj[1]['rotmat'] = np.dot(
lftlj[1]['refcs'],
rm.rodrigues(lftlj[1]['rotax'], lftlj[1]['rotangle']))
lftlj[1]['linkend'] = np.dot(lftlj[1]['rotmat'],
lftlj[1]['linkvec']) + lftlj[1]['linkpos']
lftlj[1]['rngmin'] = -(360 - rngsafemargin)
lftlj[1]['rngmax'] = +(360 - rngsafemargin)
# the 2nd joint and link
lftlj[2]['name'] = 'link2'
lftlj[2]['mother'] = 1
lftlj[2]['child'] = 3
lftlj[2]['linkpos'] = lftlj[1]['linkend']
lftlj[2]['linkvec'] = np.array([-243.65, 92.5, 0])
lftlj[2]['rotax'] = np.array([0, -1, 0])
lftlj[2]['rotangle'] = 0
lftlj[2]['inherentR'] = np.eye(3)
lftlj[2]['refcs'] = np.dot(lftlj[1]['rotmat'], lftlj[2]['inherentR'])
lftlj[2]['rotmat'] = np.dot(
lftlj[2]['refcs'],
rm.rodrigues(lftlj[2]['rotax'], lftlj[2]['rotangle']))
lftlj[2]['linkend'] = np.dot(lftlj[2]['rotmat'],
lftlj[2]['linkvec']) + lftlj[2]['linkpos']
lftlj[2]['rngmin'] = -(360 - rngsafemargin)
lftlj[2]['rngmax'] = +(360 - rngsafemargin)
# the 3rd joint and link
lftlj[3]['name'] = 'link3'
lftlj[3]['mother'] = 2
lftlj[3]['child'] = 4
lftlj[3]['linkpos'] = lftlj[2]['linkend']
lftlj[3]['linkvec'] = np.array([-213.25, 0, 0])
lftlj[3]['rotax'] = np.array([0, -1, 0])
lftlj[3]['rotangle'] = 0
lftlj[3]['inherentR'] = np.eye(3)
lftlj[3]['refcs'] = np.dot(lftlj[2]['rotmat'], lftlj[3]['inherentR'])
lftlj[3]['rotmat'] = np.dot(
lftlj[3]['refcs'],
rm.rodrigues(lftlj[3]['rotax'], lftlj[3]['rotangle']))
lftlj[3]['linkend'] = np.dot(lftlj[3]['rotmat'],
lftlj[3]['linkvec']) + lftlj[3]['linkpos']
lftlj[3]['rngmin'] = -(360 - rngsafemargin)
lftlj[3]['rngmax'] = +(360 - rngsafemargin)
# the 4th joint and link
lftlj[4]['name'] = 'link4'
lftlj[4]['mother'] = 3
lftlj[4]['child'] = 5
lftlj[4]['linkpos'] = lftlj[3]['linkend']
lftlj[4]['linkvec'] = np.array([0, -85.35, 0])
lftlj[4]['rotax'] = np.array([0, -1, 0])
lftlj[4]['rotangle'] = 0
lftlj[4]['inherentR'] = np.eye(3)
lftlj[4]['refcs'] = np.dot(lftlj[3]['rotmat'], lftlj[4]['inherentR'])
lftlj[4]['rotmat'] = np.dot(
lftlj[4]['refcs'],
rm.rodrigues(lftlj[4]['rotax'], lftlj[4]['rotangle']))
lftlj[4]['linkend'] = np.dot(lftlj[4]['rotmat'],
lftlj[4]['linkvec']) + lftlj[4]['linkpos']
lftlj[4]['rngmin'] = -(360 - rngsafemargin)
lftlj[4]['rngmax'] = +(360 - rngsafemargin)
# the 5th joint and link
lftlj[5]['name'] = 'link5'
lftlj[5]['mother'] = 4
lftlj[5]['child'] = 6
lftlj[5]['linkpos'] = lftlj[4]['linkend']
lftlj[5]['linkvec'] = np.array([0, -81.9, -85])
lftlj[5]['rotax'] = np.array([0, 0, -1])
lftlj[5]['rotangle'] = 0
lftlj[5]['inherentR'] = np.eye(3)
lftlj[5]['refcs'] = np.dot(lftlj[4]['rotmat'], lftlj[5]['inherentR'])
lftlj[5]['rotmat'] = np.dot(
lftlj[5]['refcs'],
rm.rodrigues(lftlj[5]['rotax'], lftlj[5]['rotangle']))
lftlj[5]['linkend'] = np.dot(lftlj[5]['rotmat'],
lftlj[5]['linkvec']) + lftlj[5]['linkpos']
lftlj[5]['rngmin'] = -(360 - rngsafemargin)
lftlj[5]['rngmax'] = +(360 - rngsafemargin)
# the 6th joint and link
lftlj[6]['name'] = 'link6'
lftlj[6]['mother'] = 5
lftlj[6]['child'] = -1
lftlj[6]['linkpos'] = lftlj[5]['linkend']
lftlj[6]['linkvec'] = np.array([0, 0, 195.0])
lftlj[6]['rotax'] = np.array([0, 0, 1])
lftlj[6]['rotangle'] = 0
lftlj[6]['inherentR'] = rm.rodrigues([1, 0, 0], 90)
lftlj[6]['refcs'] = np.dot(lftlj[5]['rotmat'], lftlj[6]['inherentR'])
lftlj[6]['rotmat'] = np.dot(
lftlj[6]['refcs'],
rm.rodrigues(lftlj[6]['rotax'], lftlj[6]['rotangle']))
lftlj[6]['linkend'] = np.dot(lftlj[6]['rotmat'],
lftlj[6]['linkvec']) + lftlj[6]['linkpos']
lftlj[6]['rngmin'] = -(360 - rngsafemargin)
lftlj[6]['rngmax'] = +(360 - rngsafemargin)
return lftlj
return bulletboxesnode
def genBulletCDBox(obstaclecm, name='autogen'):
"""
generate a bullet cd obj using a list of AABB box generated from obstaclenp
:param obstaclenp: the AABB box of the obstaclenp will be computed and used for cd
:return: bulletrigidbody
author: weiwei
date: 20190126, osaka
"""
bulletboxesnode = BulletRigidBodyNode(name)
bulletboxshape = BulletBoxShape.makeFromSolid(obstaclecm.cdcn.getSolid(0))
bulletboxesnode.addShape(bulletboxshape,
TransformState.makeMat(obstaclecm.getMat()))
return bulletboxesnode
if __name__ == "main":
import numpy as np
import pandaplotutils.pandageom as pg
import trimesh.primitives as tp
import utiltools.robotmath as rm
tribox = pg.trimeshToPanda(
tp.Box(box_center=np.array([0, 0, 0]),
box_transform=rm.rodrigues([0, 0, 1], 30)))
if __name__ == '__main__':
import robothelper
import utiltools.misc.p3dhtils as p3dh
import environment.collisionmodel as cm
import manipulation.grip.freegrip as fg
import manipulation.grip.yumiintegrated.yumiintegrated as yi
import trimesh
pg = PcdGrab()
rhx = robothelper.RobotHelperX(usereal=True, startworld=True)
eepos = np.array([400, 0, 200])
eerot = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]).T
armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot, armname="rgt")
rhx.movetox(armjnts, "rgt")
nppcd1 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=1) - eepos
eerot2 = np.dot(rm.rodrigues([0, 1, 0], 90), eerot)
armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot2, armname="rgt")
rhx.movetox(armjnts, "rgt")
nppcd2 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=2) - eepos
eerot3 = np.dot(rm.rodrigues([0, 1, 0], 180), eerot)
armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot3, armname="rgt")
rhx.movetox(armjnts, "rgt")
nppcd3 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=3) - eepos
eerot4 = np.dot(rm.rodrigues([0, 1, 0], 270), eerot)
armjnts = rhx.movetoposrot(eepos=eepos, eerot=eerot4, armname="rgt")
rhx.movetox(armjnts, "rgt")
nppcd4 = pg.capturecorrectedpcd(pxc=rhx.pxc, id=4) - eepos
mergednppcd = nppcd4
mergedlistnrmls = [[0, 0, 1]] * len(mergednppcd)
mergednppcd = o3dh.merge_pcd(mergednppcd,
def __init__(self, jawwidth=50, hndcolor=None, ftsensoroffset=52):
"""
load the robotiq85 model, set jawwidth and return a nodepath
## input
pandabase:
the showbase() object
jawwidth:
the distance between fingertips
ftsensoroffset:
the offset for ftsensor
## output
sck918np:
the nodepath of this rtq85 hand
author: wangyan
date: 20190413
"""
self.sck918np = NodePath("sck918hnd")
self.handnp = self.sck918np
self.__ftsensoroffset = ftsensoroffset
self.jawwidth = jawwidth
this_dir, this_filename = os.path.split(__file__)
sck918basepath = Filename.fromOsSpecific(
os.path.join(this_dir, "schunk918egg", "sck918_base.egg"))
sck918sliderpath = Filename.fromOsSpecific(
os.path.join(this_dir, "schunk918egg", "sck918_slider.egg"))
sck918gripperpath = Filename.fromOsSpecific(
os.path.join(this_dir, "schunk918egg", "sck918_gripper.egg"))
sck918base = NodePath("sck918base")
sck918lslider = NodePath("sck918lslider")
sck918rslider = NodePath("sck918rslider")
sck918lgripper = NodePath("sck918lgripper")
sck918rgripper = NodePath("sck918rgripper")
# loader is a global variable defined by panda3d
sck918_basel = loader.loadModel(sck918basepath)
sck918_sliderl = loader.loadModel(sck918sliderpath)
sck918_gripperl = loader.loadModel(sck918gripperpath)
# base
sck918_basel.instanceTo(sck918base)
if hndcolor is None:
# rtq85base.setColor(.2,.2,.2,1)
pass
else:
sck918base.setColor(hndcolor[0], hndcolor[1], hndcolor[2],
hndcolor[3])
sck918base.setTransparency(TransparencyAttrib.MAlpha)
# sck918base.setColor(.2, .2, .2, 1.0)
sck918base.setHpr(90, -90, 0)
# left and right outer knuckle
sck918_sliderl.instanceTo(sck918lslider)
sck918lslider.setColor(.1, .1, .1, 1.0)
sck918lslider.setPos(-10, 40, 73)
sck918lslider.setHpr(-90, 0, 0)
sck918lslider.reparentTo(sck918base)
sck918_sliderl.instanceTo(sck918rslider)
sck918rslider.setColor(.1, .1, .1, 1.0)
sck918rslider.setPos(10, -40, 73)
sck918rslider.setHpr(90, 0, 0)
sck918rslider.reparentTo(sck918base)
# left and right finger
sck918_gripperl.instanceTo(sck918lgripper)
sck918lgripper.setColor(.7, .7, .7, 1.0)
sck918lgripper.setPos(-8, 20, 0)
sck918lgripper.setHpr(0, 180, 0)
sck918lgripper.reparentTo(sck918lslider)
sck918_gripperl.instanceTo(sck918rgripper)
sck918rgripper.setColor(.7, .7, .7, 1.0)
sck918rgripper.setPos(-8, 20, 0)
sck918rgripper.setHpr(0, 180, 0)
sck918rgripper.reparentTo(sck918rslider)
# rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np)
# the default x direction is facing the ee, the default z direction is facing downward
# execute this file to see the default pose
sck918base.setTransparency(TransparencyAttrib.MAlpha)
sck918base.setMat(
pandageom.npToMat4(rm.rodrigues([0, 0, 1], 90)) *
pandageom.npToMat4(rm.rodrigues([1, 0, 0], 90)) *
sck918base.getMat())
sck918base.setPos(0, 0, self.__ftsensoroffset)
sck918base.reparentTo(self.sck918np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 50.0
self.__jawwidthclosed = 0.0
def __init__(self, jawwidth=85, ftsensoroffset=52):
'''
load the robotiq85 model, set jawwidth and return a nodepath
the rtq85 gripper is composed of a parallelism and a fixed triangle,
the parallelism: 1.905-1.905; 5.715-5.715; 70/110 degree
the triangle: 4.75 (finger) 5.715 (inner knuckle) 3.175 (outer knuckle)
## input
pandabase:
the showbase() object
jawwidth:
the distance between fingertips
ftsensoroffset:
the offset for ftsensor
## output
rtq85np:
the nodepath of this rtq85 hand
author: weiwei
date: 20160627
'''
self.rtq85np = NodePath("rtq85hnd")
self.handnp = self.rtq85np
self.__ftsensoroffset = ftsensoroffset
self.jawwidth = jawwidth
this_dir, this_filename = os.path.split(__file__)
rtq85basepath = Filename.fromOsSpecific(
os.path.join(this_dir, "rtq85egg", "robotiq_85_base_link.egg"))
rtq85fingerpath = Filename.fromOsSpecific(
os.path.join(this_dir, "rtq85egg", "robotiq_85_finger_link.egg"))
rtq85fingertippath = Filename.fromOsSpecific(
os.path.join(this_dir, "rtq85egg",
"robotiq_85_finger_tip_link.egg"))
rtq85innerknucklepath = Filename.fromOsSpecific(
os.path.join(this_dir, "rtq85egg",
"robotiq_85_inner_knuckle_link.egg"))
rtq85knucklepath = Filename.fromOsSpecific(
os.path.join(this_dir, "rtq85egg", "robotiq_85_knuckle_link.egg"))
rtq85base = NodePath("rtq85base")
rtq85lknuckle = NodePath("rtq85lknuckle")
rtq85rknuckle = NodePath("rtq85rknuckle")
rtq85lfgr = NodePath("rtq85lfgr")
rtq85rfgr = NodePath("rtq85rfgr")
rtq85ilknuckle = NodePath("rtq85ilknuckle")
rtq85irknuckle = NodePath("rtq85irknuckle")
rtq85lfgrtip = NodePath("rtq85lfgrtip")
rtq85rfgrtip = NodePath("rtq85rfgrtip")
# loader is a global variable defined by panda3d
rtq85_basel = loader.loadModel(rtq85basepath)
rtq85_fingerl = loader.loadModel(rtq85fingerpath)
rtq85_fingertipl = loader.loadModel(rtq85fingertippath)
rtq85_innerknucklel = loader.loadModel(rtq85innerknucklepath)
rtq85_knucklel = loader.loadModel(rtq85knucklepath)
# base
rtq85_basel.instanceTo(rtq85base)
# left and right outer knuckle
rtq85_knucklel.instanceTo(rtq85lknuckle)
rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0)
rtq85lknuckle.setHpr(0, 0, 180)
rtq85lknuckle.reparentTo(rtq85base)
rtq85_knucklel.instanceTo(rtq85rknuckle)
rtq85rknuckle.setPos(30.60114443, 54.90451627, 0)
rtq85rknuckle.setHpr(0, 0, 0)
rtq85rknuckle.reparentTo(rtq85base)
# left and right finger
rtq85_fingerl.instanceTo(rtq85lfgr)
rtq85lfgr.setPos(31.48504435, -4.08552455, 0)
rtq85lfgr.reparentTo(rtq85lknuckle)
rtq85_fingerl.instanceTo(rtq85rfgr)
rtq85rfgr.setPos(31.48504435, -4.08552455, 0)
rtq85rfgr.reparentTo(rtq85rknuckle)
# left and right inner knuckle
rtq85_innerknucklel.instanceTo(rtq85ilknuckle)
rtq85ilknuckle.setPos(-12.7, 61.42, 0)
rtq85ilknuckle.setHpr(0, 0, 180)
rtq85ilknuckle.reparentTo(rtq85base)
rtq85_innerknucklel.instanceTo(rtq85irknuckle)
rtq85irknuckle.setPos(12.7, 61.42, 0)
rtq85irknuckle.setHpr(0, 0, 0)
rtq85irknuckle.reparentTo(rtq85base)
# left and right fgr tip
rtq85_fingertipl.instanceTo(rtq85lfgrtip)
rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0)
rtq85lfgrtip.reparentTo(rtq85ilknuckle)
rtq85_fingertipl.instanceTo(rtq85rfgrtip)
rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0)
rtq85rfgrtip.setHpr(0, 0, 0)
rtq85rfgrtip.reparentTo(rtq85irknuckle)
# rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np)
# the default x direction is facing the ee, the default z direction is facing downward
# execute this file to see the default pose
rtq85base.setMat(
base.pg.npToMat4(rm.rodrigues([1, 0, 0], 90)) * rtq85base.getMat())
rtq85base.setPos(0, 0, self.__ftsensoroffset)
rtq85base.reparentTo(self.rtq85np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 85.0
self.__jawwidthclosed = 0.0
def findtubes(self, tubestand_homomat, tgtpcdnp, toggledebug=False):
"""
:param tgtpcdnp:
:return:
author: weiwei
date: 20200317
"""
elearray = np.zeros((5, 10))
eleconfidencearray = np.zeros((5, 10))
tgtpcdnp = o3dh.remove_outlier(tgtpcdnp,
downsampling_voxelsize=None,
nb_points=90,
radius=5)
# transform back to the local frame of the tubestand
tgtpcdnp_normalized = rm.homotransformpointarray(
rm.homoinverse(tubestand_homomat), tgtpcdnp)
if toggledebug:
cm.CollisionModel(tgtpcdnp_normalized).reparentTo(base.render)
tscm2 = copy.deepcopy(self.tubestandcm)
tscm2.reparentTo(base.render)
for i in range(5):
for j in range(10):
holepos = self.tubeholecenters[i][j]
tmppcd = self._crop_pcd_overahole(tgtpcdnp_normalized,
holepos[0], holepos[1])
if len(tmppcd) > 50:
if toggledebug:
print(
"------more than 50 raw points, start a new test------"
)
tmppcdover100 = tmppcd[tmppcd[:, 2] > 100]
tmppcdbelow90 = tmppcd[tmppcd[:, 2] < 90]
tmppcdlist = [tmppcdover100, tmppcdbelow90]
if toggledebug:
print("rotate around...")
rejflaglist = [False, False]
allminstdlist = [[], []]
newtmppcdlist = [None, None]
minstdlist = [None, None]
for k in range(2):
if toggledebug:
print("checking over 100 and below 90, now: ", j)
if len(tmppcdlist[k]) < 10:
rejflaglist[k] = True
continue
for angle in np.linspace(0, 180, 10):
tmphomomat = np.eye(4)
tmphomomat[:3, :3] = rm.rodrigues(
tubestand_homomat[:3, 2], angle)
newtmppcdlist[k] = rm.homotransformpointarray(
tmphomomat, tmppcdlist[k])
minstdlist[k] = np.min(
np.std(newtmppcdlist[k][:, :2], axis=0))
if toggledebug:
print(minstdlist[k])
allminstdlist[k].append(minstdlist[k])
if minstdlist[k] < 1.5:
rejflaglist[k] = True
if toggledebug:
print("rotate round done")
print("minstd ",
np.min(np.asarray(allminstdlist[k])))
if all(item for item in rejflaglist):
continue
elif all(not item for item in rejflaglist):
print("CANNOT tell if the tube is big or small")
raise ValueError()
else:
tmppcd = tmppcdbelow90 if rejflaglist[
0] else tmppcdover100
candidatetype = 2 if rejflaglist[0] else 1
tmpangles = np.arctan2(tmppcd[:, 1], tmppcd[:, 0])
tmpangles[
tmpangles < 0] = 360 + tmpangles[tmpangles < 0]
if toggledebug:
print(np.std(tmpangles))
print("ACCEPTED! ID: ", i, j)
elearray[i][j] = candidatetype
eleconfidencearray[i][j] = 1
if toggledebug:
# normalized
objnp = p3dh.genpointcloudnodepath(tmppcd,
pntsize=5)
rgb = np.random.rand(3)
objnp.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp.reparentTo(base.render)
stick = p3dh.gendumbbell(
spos=np.array([holepos[0], holepos[1], 10]),
epos=np.array([holepos[0], holepos[1], 60]))
stick.setColor(rgb[0], rgb[1], rgb[2], 1)
stick.reparentTo(base.render)
# original
tmppcd_tr = rm.homotransformpointarray(
tubestand_homomat, tmppcd)
objnp_tr = p3dh.genpointcloudnodepath(tmppcd_tr,
pntsize=5)
objnp_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
objnp_tr.reparentTo(base.render)
spos_tr = rm.homotransformpoint(
tubestand_homomat,
np.array([holepos[0], holepos[1], 0]))
stick_tr = p3dh.gendumbbell(
spos=np.array([spos_tr[0], spos_tr[1], 10]),
epos=np.array([spos_tr[0], spos_tr[1], 60]))
stick_tr.setColor(rgb[0], rgb[1], rgb[2], 1)
stick_tr.reparentTo(base.render)
# box normalized
center, bounds = rm.get_aabb(tmppcd)
boxextent = np.array([
bounds[0, 1] - bounds[0, 0],
bounds[1, 1] - bounds[1, 0],
bounds[2, 1] - bounds[2, 0]
])
boxhomomat = np.eye(4)
boxhomomat[:3, 3] = center
box = p3dh.genbox(
extent=boxextent,
homomat=boxhomomat,
rgba=np.array([rgb[0], rgb[1], rgb[2], .3]))
box.reparentTo(base.render)
# box original
center_r = rm.homotransformpoint(
tubestand_homomat, center)
boxhomomat_tr = copy.deepcopy(tubestand_homomat)
boxhomomat_tr[:3, 3] = center_r
box_tr = p3dh.genbox(
extent=boxextent,
homomat=boxhomomat_tr,
rgba=np.array([rgb[0], rgb[1], rgb[2], .3]))
box_tr.reparentTo(base.render)
if toggledebug:
print("------the new test is done------")
return elearray, eleconfidencearray
def __init__(self, jawwidth=85, hndcolor=None, ftsensoroffset = 52):
'''
load the robotiq85 model, set jawwidth and return a nodepath
the rtq85 gripper is composed of a parallelism and a fixed triangle,
the parallelism: 1.905-1.905; 5.715-5.715; 70/110 degree
the triangle: 4.75 (finger) 5.715 (inner knuckle) 3.175 (outer knuckle)
NOTE: the setColor function is only useful when the models dont have any materials
## input
pandabase:
the showbase() object
jawwidth:
the distance between fingertips
ftsensoroffset:
the offset for ftsensor
## output
rtq85np:
the nodepath of this rtq85 hand
author: weiwei
date: 20160627
'''
self.rtq85np = NodePath("rtq85hnd")
self.jawwidth = jawwidth
self.__ftsensoroffset = ftsensoroffset
this_dir, this_filename = os.path.split(__file__)
rtq85basepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_base_link_nm.egg"))
rtq85fingerpath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_link_nm.egg"))
rtq85fingertippath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_finger_tip_link_nm.egg"))
rtq85innerknucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_inner_knuckle_link_nm.egg"))
rtq85knucklepath = Filename.fromOsSpecific(os.path.join(this_dir, "rtq85egg/nomat", "robotiq_85_knuckle_link_nm.egg"))
rtq85base = NodePath("rtq85base")
rtq85lknuckle = NodePath("rtq85lknuckle")
rtq85rknuckle = NodePath("rtq85rknuckle")
rtq85lfgr = NodePath("rtq85lfgr")
rtq85rfgr = NodePath("rtq85rfgr")
rtq85ilknuckle = NodePath("rtq85ilknuckle")
rtq85irknuckle = NodePath("rtq85irknuckle")
rtq85lfgrtip = NodePath("rtq85lfgrtip")
rtq85rfgrtip = NodePath("rtq85rfgrtip")
# loader is a global variable defined by panda3d
rtq85_basel = loader.loadModel(rtq85basepath)
rtq85_fingerl = loader.loadModel(rtq85fingerpath)
rtq85_fingertipl = loader.loadModel(rtq85fingertippath)
rtq85_innerknucklel = loader.loadModel(rtq85innerknucklepath)
rtq85_knucklel = loader.loadModel(rtq85knucklepath)
# base
rtq85_basel.instanceTo(rtq85base)
if hndcolor is None:
# rtq85base.setColor(.2,.2,.2,1)
pass
else:
rtq85base.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85base.setTransparency(TransparencyAttrib.MAlpha)
# left and right outer knuckle
rtq85_knucklel.instanceTo(rtq85lknuckle)
rtq85lknuckle.setPos(-30.60114443, 54.90451627, 0)
rtq85lknuckle.setHpr(0, 0, 180)
if hndcolor is None:
# rtq85lknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85lknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85lknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85lknuckle.reparentTo(rtq85base)
rtq85_knucklel.instanceTo(rtq85rknuckle)
rtq85rknuckle.setPos(30.60114443, 54.90451627, 0)
rtq85rknuckle.setHpr(0, 0, 0)
if hndcolor is None:
# rtq85rknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85rknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85rknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85rknuckle.reparentTo(rtq85base)
# left and right finger
rtq85_fingerl.instanceTo(rtq85lfgr)
rtq85lfgr.setPos(31.48504435, -4.08552455, 0)
if hndcolor is None:
# rtq85lfgr.setColor(0.2,0.2,0.2,1)
pass
else:
rtq85lfgr.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85lfgr.setTransparency(TransparencyAttrib.MAlpha)
rtq85lfgr.reparentTo(rtq85lknuckle)
rtq85_fingerl.instanceTo(rtq85rfgr)
rtq85rfgr.setPos(31.48504435, -4.08552455, 0)
if hndcolor is None:
# rtq85rfgr.setColor(0.2,0.2,0.2,1)
pass
else:
rtq85rfgr.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85rfgr.setTransparency(TransparencyAttrib.MAlpha)
rtq85rfgr.reparentTo(rtq85rknuckle)
# left and right inner knuckle
rtq85_innerknucklel.instanceTo(rtq85ilknuckle)
rtq85ilknuckle.setPos(-12.7, 61.42, 0)
rtq85ilknuckle.setHpr(0, 0, 180)
if hndcolor is None:
# rtq85ilknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85ilknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85ilknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85ilknuckle.reparentTo(rtq85base)
rtq85_innerknucklel.instanceTo(rtq85irknuckle)
rtq85irknuckle.setPos(12.7, 61.42, 0)
rtq85irknuckle.setHpr(0, 0, 0)
if hndcolor is None:
# rtq85irknuckle.setColor(.5,.5,.5,1)
pass
else:
rtq85irknuckle.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85irknuckle.setTransparency(TransparencyAttrib.MAlpha)
rtq85irknuckle.reparentTo(rtq85base)
# left and right fgr tip
rtq85_fingertipl.instanceTo(rtq85lfgrtip)
rtq85lfgrtip.setPos(37.59940821, 43.03959807, 0)
if hndcolor is None:
# rtq85lfgrtip.setColor(.5,.5,.5,1)
pass
else:
rtq85lfgrtip.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85lfgrtip.setTransparency(TransparencyAttrib.MAlpha)
rtq85lfgrtip.reparentTo(rtq85ilknuckle)
rtq85_fingertipl.instanceTo(rtq85rfgrtip)
rtq85rfgrtip.setPos(37.59940821, 43.03959807, 0)
rtq85rfgrtip.setHpr(0, 0, 0)
if hndcolor is None:
# rtq85rfgrtip.setColor(.5,.5,.5,1)
pass
else:
rtq85rfgrtip.setColor(hndcolor[0],hndcolor[1],hndcolor[2],hndcolor[3])
rtq85rfgrtip.setTransparency(TransparencyAttrib.MAlpha)
rtq85rfgrtip.reparentTo(rtq85irknuckle)
# rotate to x, y, z coordinates (this one rotates the base, not the self.rtq85np)
rtq85base.setMat(pandageom.npToMat4(rm.rodrigues([1,0,0], 90))*rtq85base.getMat())
rtq85base.setPos(0,0,self.__ftsensoroffset)
rtq85base.reparentTo(self.rtq85np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 85.0
self.__jawwidthclosed = 0.0
base = pandactrl.World(camp=[2700, 300, 2700], lookatp=[0, 0, 1000])
env = Env()
env.reparentTo(base.render)
objcm = env.loadobj("bunnysim.stl")
objcm.setColor(.2, .5, 0, 1)
objcm.setPos(400, -200, 1200)
objcm.reparentTo(base.render)
objcm.showcn()
obscmlist = env.getstationaryobslist()
for obscm in obscmlist:
obscm.showcn()
objpos = np.array([400, -300, 1200])
objrot = rm.rodrigues([0, 1, 0], 45)
objcm2 = env.loadobj("housing.stl")
objcm2.setColor(1, .5, 0, 1)
env.addchangableobs(base.render, objcm2, objpos, objrot)
robotsim = robotsim.NxtRobot()
rgthnd = rtq85nm.newHand(hndid="rgt", ftsensoroffset=0)
lfthnd = rtq85nm.newHand(hndid="lft", ftsensoroffset=0)
robotmeshgen = robotmesh.NxtMesh(rgthand=rgthnd, lfthand=lfthnd)
robotmesh = robotmeshgen.genmnp(robotsim, toggleendcoord=False)
robotmesh.reparentTo(base.render)
base.pggen.plotAxis(base.render, spos=Vec3(400, -300, 900))
base.pggen.plotAxis(base.render, spos=Vec3(300, 0, 1300))
base.run()
base.pggen.plotAxis(base.render, length=30, thickness=2)
objpath = "./objects/tubelarge.stl"
obj = cm.CollisionModel(objinit=objpath)
obj.setColor(.8, .6, .3, .5)
obj.reparentTo(base.render)
hndfa = yi.YumiIntegratedFactory()
pregrasps = []
yihnd = hndfa.genHand()
c0nvec = np.array([0, -1, 0])
approachvec = np.array([1, 0, 0])
for z in [60, 90]:
for anglei in [0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5]:
newcv = np.dot(rm.rodrigues((0, 0, 1), anglei), c0nvec)
tempav = np.dot(rm.rodrigues((0, 0, 1), anglei), approachvec)
for anglej in [
0, 22.5, 45, 67.5, 90, 112.5, 135, 157.5, 180, 202.5, 225,
247.5, 270, 292.5, 315, 337.5
]:
newyihand = yihnd.copy()
newav = np.dot(rm.rodrigues(newcv, anglej), tempav)
base.pggen.plotAxis(newyihand.hndnp, length=15, thickness=2)
pregrasps.append(
newyihand.approachAt(0,
0,
z,
newcv[0],
newcv[1],
newcv[2],
def phoxi_computeeeinphx(yhx,
pxc,
armname,
actionpos,
actionrot,
parameters,
aruco_dict,
criteriaradius=None):
"""
:param yhx:
:param pxc:
:param armname:
:param actionpos:
:param actionrot:
:param parameters:
:param aruco_dict:
:param criteriaradius: rough radius used to determine if the newly estimated center is correct or not
:return:
author: weiwei
date: 20190110
"""
def fitfunc(p, coords):
x0, y0, z0, R = p
x, y, z = coords.T
return np.sqrt((x - x0)**2 + (y - y0)**2 + (z - z0)**2)
errfunc = lambda p, x: fitfunc(p, x) - p[3]
coords = []
rangex = [np.array([1, 0, 0]), [-30, -15, 0, 15, 30]]
rangey = [np.array([0, 1, 0]), [-30, -15, 15, 30]]
rangez = [np.array([0, 0, 1]), [-90, -60, -30, 30, 60]]
rangeaxis = [rangex, rangey, rangez]
lastarmjnts = yhx.robot_s.initrgtjnts
for axisid in range(3):
axis = rangeaxis[axisid][0]
for angle in rangeaxis[axisid][1]:
goalpos = actionpos
goalrot = np.dot(rm.rodrigues(axis, angle), actionrot)
armjnts = yhx.movetoposrotmsc(eepos=goalpos,
eerot=goalrot,
msc=lastarmjnts,
armname=armname)
if armjnts is not None and not yhx.pcdchecker.isSelfCollided(
yhx.robot_s):
lastarmjnts = armjnts
yhx.movetox(armjnts, armname=armname)
pxc.triggerframe()
img = pxc.gettextureimg()
pcd = pxc.getpcd()
phxpos = getcenter(img, pcd, aruco_dict, parameters)
if phxpos is not None:
coords.append(phxpos)
print(coords)
if len(coords) < 3:
return [None, None]
# for coord in coords:
# yhx.p3dh.gensphere(coord, radius=5).reparentTo(base.render)
coords = np.asarray(coords)
# try:
initialguess = np.ones(4)
initialguess[:3] = np.mean(coords, axis=0)
finalestimate, flag = leastsq(errfunc, initialguess, args=(coords, ))
if len(finalestimate) == 0:
return [None, None]
print(finalestimate)
print(np.linalg.norm(coords - finalestimate[:3], axis=1))
# yhx.p3dh.gensphere(finalestimate[:3], rgba=np.array([0,1,0,1]), radius=5).reparentTo(base.render)
# yhx.base.run()
# except:
# return [None, None]
if criteriaradius is not None:
if abs(finalestimate[3] - criteriaradius) > 5:
return [None, None]
return np.array(finalestimate[:3]), finalestimate[3]
objstpos = np.array([354.5617667638, -256.0889005661, 1090.5])
objstrot = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]).T
objgpos = np.array([304.5617667638, -277.1026725769, 1101.0])
objgrot = np.array([[1.0, 0.0, 0.0], [0.0, 6.12323426293e-17, -1.0],
[0.0, 1.0, 6.12323426293e-17]]).T
objcmcopys = copy.deepcopy(objcm)
objcmcopys.setColor(0.0, 0.0, 1.0, .3)
objcmcopys.setMat(base.pg.npToMat4(npmat3=objstrot, npvec3=objstpos))
objcmcopys.reparentTo(base.render)
startpos = np.array([354.5617667638, -256.0889005661, 1060.5])
startrot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]).T
goalpos3 = np.array([354.5617667638, -256.0889005661, 1150.5])
goalrot3 = np.array([[1, 0, 0], [0, -0.92388, -0.382683],
[0, 0.382683, -0.92388]]).T
goalrot3 = np.dot(rm.rodrigues([1, 0, 0], 20), goalrot3)
goalpos3 = goalpos3 - 70.0 * goalrot3[:, 2]
goalpos4 = np.array([154.5617667638, -500, 1050.5])
goalrot4 = np.dot(rm.rodrigues([1, 0, 0], -90), goalrot3)
goalpos4 = goalpos4 - 150.0 * goalrot4[:, 2]
# goalpos2 = goalpos2 - 150.0*goalrot2[:,2]
start = robot.numik(startpos, startrot, armname)
goal3 = robot.numikmsc(goalpos3, goalrot3, msc=start, armname=armname)
print(goal3)
goal4 = robot.numikmsc(goalpos4, goalrot4, msc=start, armname=armname)
print(goal4)
planner = rrtc.RRTConnect(start=goal3,
goal=goal4,
ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
reduceradius=30,
discretesize=8,
torqueresist=100)
# freesuctst.showfacets(togglesamples=True, togglenormals=False,
# togglesamples_ref=True, togglenormals_ref=False,
# togglesamples_refcls=True, togglenormals_refcls=False, specificfacet=True)
p3dh.gensphere(pos=np.mean(freesuctst.objcm.objtrm.vertices, axis=0),
radius=5,
rgba=[1, 1, 1, 1]).reparentTo(rhx.base.render)
print(len(freesuctst.sucrotmats_planned))
print(len(freesuctst.facets))
for i, homomat in enumerate(freesuctst.sucrotmats_planned):
# homomat[:3,3] = homomat[:3,3]-homomat[:3,2]*120
homomatnew = np.copy(homomat)
homomatnew[:3, 3] = homomat[:3, 3] - homomat[:3, 2] * 3
homomatnew[:3, :3] = np.dot(rm.rodrigues(homomat[:3, 0], 45),
homomat[:3, :3])
# tmpef = effa.genendeffector()
# tmpef.sethomomat(homomat)
# tmpef.reparentTo(rhx.base.render)
# tmpef.setcolor(1, 1, 1, .3)
armjnts = rhx.movetopose(homomatnew, "lft")
if armjnts is not None:
# if i == 1:
tmpef = effa.genendeffector()
tmpef.set_homomat(homomat)
tmpef.reparentTo(rhx.base.render)
tmpef.set_rgba(1, 1, 1, .3)
pos = homomat[:3, 3]
rot = homomat[:3, :3]
rhx.opengripperx("lft")
def gencm(self, startposobj, startrotobj, startposrbt, startrotrbt, isrotated = False, iscornered = False, isdrooped = False, isinclined = False):
"""
make the collision model of the object, set its pose according to the robot end effector pose
:param startposobj: object position
:param startrotobj: object orientation
:param startposrbt: robot position
:param startrotrbt: robot orientation
:param isrotated: is the object to be rotated 90 degrees about the vertical axis of the world frame
:param iscornered:is the object grasped from its corner
:param isdrooped: is the object drooped due to orientation
:param isinclined: is the object at an inclined pose
:return: collision model of the object with set pose
"""
#load the object and set its pose related to the robot pose
startposobj = startposobj
startrotobj = startrotobj
startpos = startposrbt
startrot = startrotrbt
#create the collision model
self.objcm = cm.CollisionModel(objinit=self.objpath)
# if required to flip the object and the robot pose
# startrot = rot_transform(startrot, 180)
# set the object pose in hand
# easy to be automated for planning
if isrotated:
self.torque = 9.81 * self.m * self.width / 2 / 1000 # Nm
print("rotated torque is set!")
print("self.torque is:", self.torque)
rotz = rm.rodrigues([0, 0, 1], -90)
tmp_rot = np.dot(rotz, startrotobj)
startrotobj = np.dot(tmp_rot, startrotobj)
startposobj[0] -= (self.length/2-self.width/2)
if isinclined:
incline_angle = 10
tmp_incline_rot = rm.rodrigues(startrot[:,2],incline_angle)
startrot = np.dot(tmp_incline_rot,startrot)
startrotobj = np.dot(tmp_incline_rot, startrotobj)
objcmcopy = copy.deepcopy(self.objcm)
objcmcopy.setMat(pg.npToMat4(startrotobj,startposobj))
##for visualization of the limit
# for incangle in range(0,20,10):
# print (incangle)
# tmp_incline_rot = rm.rodrigues(startrot[:, 2], incangle)
# startrotobjinc = np.dot(tmp_incline_rot, startrotobj)
# objcmcopy = copy.deepcopy(objcm)
# startrotrbt = np.dot(tmp_incline_rot, startrot)
# rbtangles = rbt.numik(startpos, startrotrbt, "lft")
# rbt.movearmfk(rbtangles, "lft")
# rbtmg.genmnp(rbt, jawwidthlft=objheight,toggleendcoord=False).reparentTo(base.render)
# objcmcopy.setMat(pg.npToMat4(startrotobjinc, startposobj))
# if incangle == 10:
# objcmcopy.setColor(.8, .0, .0, .5)
# objcmcopy.reparentTo(base.render)
# base.run()
if iscornered:
corner_angle = 45
rotz = rm.rodrigues([0,0,1],corner_angle)
tmp_rot = np.dot(rotz,startrotobj)
startrotobj = np.dot(tmp_rot,startrotobj)
grasppnt = [-self.length/2, self.width/2,0,1]
obj_w_t = np.eye(4)
obj_w_t[:3,:3] = startrotobj
obj_w_t[:3,3] = startposobj
print("obj_w_t",obj_w_t)
grasppnt_w = np.dot(obj_w_t,grasppnt)
print(grasppnt_w)
print(startpos)
pos_diff = [a-b for a,b in zip(grasppnt_w,startpos)]
print ("Position difference is: ", pos_diff)
print("Position difference is: ", [grasppnt_w[0],grasppnt_w[1],grasppnt_w[2]]-startpos)
startposobj-=pos_diff
if isdrooped == True:
droop_angle = -20
tmp_rot_ee_x = rm.rodrigues(startrot[:,0],droop_angle)
startrotobj = np.dot(tmp_rot_ee_x,startrotobj)
# grasppnt = [-objlength/2*np.cos(droop_angle), objwidth/2*np.sin(droop_angle), 0, 1]
# obj_w_t = np.eye(4)
# obj_w_t[:3, :3] = startrotobj
# obj_w_t[:3, 3] = startposobj
# grasppnt_w = np.dot(obj_w_t, grasppnt)
# print(grasppnt_w)
# print(startpos)
# pos_diff = [a - b for a, b in zip(grasppnt_w, startpos)]
# print("Position difference is: ", pos_diff)
# print("Position difference is: ", [grasppnt_w[0], grasppnt_w[1], grasppnt_w[2]] - startpos)
# startposobj += pos_diff
self.objcm.setMat(pg.npToMat4(startrotobj,startposobj))
return self.objcm
