def movewaist(self, rotangle=0):
"""
rotate the base of the robot
:param rotangle: in degree
:return: null
author: weiwei
date: 20170410
"""
# right arm
self.rgtarm[0]['rotangle'] = rotangle
self.rgtarm[0]['rotmat'] = rm.rodrigues(self.rgtarm[0]['rotax'],
self.rgtarm[0]['rotangle'])
self.rgtarm[0]['linkend'] = np.squeeze(
np.dot(self.rgtarm[0]['rotmat'], self.rgtarm[0]['linkvec'].reshape(
(-1, )))) + self.rgtarm[0]['linkpos']
# left arm
self.lftarm[0]['rotangle'] = rotangle
self.lftarm[0]['rotmat'] = rm.rodrigues(self.lftarm[0]['rotax'],
self.lftarm[0]['rotangle'])
self.lftarm[0]['linkend'] = np.squeeze(
np.dot(self.lftarm[0]['rotmat'], self.lftarm[0]['linkvec'].reshape(
(-1, )))) + self.lftarm[0]['linkpos']
self.__updatefk(self.rgtarm)
self.__updatefk(self.lftarm)
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 == 9:
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 __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 == 9:
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 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])
virtualgoalpos0 = np.array(
[411.2619717, -161.958744, 65.9594187 + 973])
virtualgoalrot0 = rm.rodrigues([
0,
1,
0,
], -90)
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 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 movewaistshoulder(self, waistrot, shoulderrot, armid="rgt"):
"""
rotate the shoulderwaist of the robot
:param waistrot: in degree
:param shoulderrot: in degree
:return: null
author: weiwei
date: 20170112
"""
if armid != "rgt" and armid != "lft":
raise ep.ValueError("Armid must be 'rgt' or 'lft'")
armlj = self.rgtarm
if armid == "lft":
armlj = self.lftarm
# right arm
self.rgtarm[0]['rotangle'] = waistrot
self.rgtarm[0]['rotmat'] = rm.rodrigues(self.rgtarm[0]['rotax'],
self.rgtarm[0]['rotangle'])
self.rgtarm[0]['linkend'] = np.squeeze(
np.dot(self.rgtarm[0]['rotmat'], self.rgtarm[0]['linkvec'].reshape(
(-1, )))) + self.rgtarm[0]['linkpos']
# left arm
self.lftarm[0]['rotangle'] = waistrot
self.lftarm[0]['rotmat'] = rm.rodrigues(self.lftarm[0]['rotax'],
self.lftarm[0]['rotangle'])
self.lftarm[0]['linkend'] = np.squeeze(
np.dot(self.lftarm[0]['rotmat'], self.lftarm[0]['linkvec'].reshape(
(-1, )))) + self.lftarm[0]['linkpos']
# shoulder
armlj[1]['rotangle'] = shoulderrot
self.__updatefk(self.rgtarm)
self.__updatefk(self.lftarm)
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'] = rm.rodrigues(self.rgtarm[0]['rotax'], self.rgtarm[0]['rotangle'])
self.rgtarm[0]['linkend'] = np.squeeze(np.dot(self.rgtarm[0]['rotmat'], self.rgtarm[0]['linkvec'].reshape((-1,))))+self.rgtarm[0]['linkpos']
self.__updatefk(self.rgtarm)
# left arm
self.lftarm[0]['rotangle'] = rotangle
self.lftarm[0]['rotmat'] = rm.rodrigues(self.lftarm[0]['rotax'], self.lftarm[0]['rotangle'])
self.lftarm[0]['linkend'] = np.squeeze(np.dot(self.lftarm[0]['rotmat'], self.lftarm[0]['linkvec'].reshape((-1,))))+self.lftarm[0]['linkpos']
self.__updatefk(self.lftarm)
def updateRbdVW(rbd, dtime):
eps = 1e-6
anglewvalue = np.linalg.norm(rbd.anglew)
if anglewvalue < eps:
rbd.pos = rbd.pos + dtime * rbd.linearv
rbd.rotmat = rbd.rotmat
else:
theta = math.degrees(anglewvalue * dtime)
waxis = rbd.anglew / anglewvalue
vnormw = rbd.linearv / anglewvalue
rotmat = rm.rodrigues(waxis, theta)
rbd.pos = np.dot(rotmat, rbd.pos) + np.dot((np.identity(3)-rotmat), np.cross(waxis, vnormw)) + \
waxis.dot(waxis.transpose())*vnormw*anglewvalue*dtime
rbd.rotmat = rotmat.dot(rbd.rotmat)
def updateRbdVW(rbd, dtime):
eps = 1e-6
anglewvalue = np.linalg.norm(rbd.anglew)
if anglewvalue < eps:
rbd.pos = rbd.pos + dtime*rbd.linearv
rbd.rotmat = rbd.rotmat
else:
theta = math.degrees(anglewvalue*dtime)
waxis = rbd.anglew/anglewvalue
vnormw = rbd.linearv/anglewvalue
rotmat = rm.rodrigues(waxis, theta)
rbd.pos = np.dot(rotmat, rbd.pos) + np.dot((np.identity(3)-rotmat), np.cross(waxis, vnormw)) + \
waxis.dot(waxis.transpose())*vnormw*anglewvalue*dtime
rbd.rotmat = rotmat.dot(rbd.rotmat)
def doEulerPhysics(rbd, dtime):
globalinerten = np.dot(rbd.rotmat,
np.dot(rbd.inertiatensor, np.transpose(rbd.rotmat)))
globalinerteninv = np.dot(
rbd.rotmat,
np.dot(np.linalg.inv(rbd.inertiatensor), np.transpose(rbd.rotmat)))
angularmomentum = np.dot(globalinerten, rbd.anglew)
rbd.danglew = np.dot(globalinerteninv,
(-np.cross(rbd.anglew, angularmomentum)))
# print rbd.danglew
print angularmomentum
# print rbd.rotmat
anglewvalue = np.linalg.norm(rbd.anglew)
axis = rbd.anglew / anglewvalue
# note rodrigues is in degree
theta = math.degrees(anglewvalue * dtime)
# rbd.rotmat = np.dot(pg.mat3ToNp(Mat4.rotateMat(theta, Vec3(axis[0], axis[1], axis[2])).getUpper3()), rbd.rotmat)
# rbd.rotmat = np.dot(tf.rotation_matrix(theta*math.pi/180.0, axis)[:3, :3], rbd.rotmat)
rbd.rotmat = np.dot(rm.rodrigues(axis, theta), rbd.rotmat)
rbd.anglew = rbd.anglew + rbd.danglew * dtime
return angularmomentum
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 saveToDB(self, positionlist, gdb, discretesize=4.0):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "(" + str(360 * i * 1.0 / discretesize) + "), "
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT tabletopplacements.idtabletopplacements, \
tabletopplacements.tabletopposition, angle.value,\
freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \
freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \
freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \
FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d AND \
freetabletopplacement.idfreetabletopplacement = %d" % (
idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
# no grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
if len(result1) > 20000:
result1 = result1[0::int(len(result1) / 20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:, 0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:, 1]]
rotanglelist = [float(x) for x in result1[:, 2]]
freegripcontactpoint0list = [
dc.strToV3(x) for x in result1[:, 3]
]
freegripcontactpoint1list = [
dc.strToV3(x) for x in result1[:, 4]
]
freegripcontactnormal0list = [
dc.strToV3(x) for x in result1[:, 5]
]
freegripcontactnormal1list = [
dc.strToV3(x) for x in result1[:, 6]
]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:, 7]]
freegripjawwidthlist = [float(x) for x in result1[:, 8]]
freegripidlist = [int(x) for x in result1[:, 9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat * rotmat4
sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements)
gdb.execute(sql)
else:
print "Tabletopgrips already exist!"
print "Save to DB done!"
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print str(self.counter) + "/" + str(self.facetpairs.shape[0] - 1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
if j == 0:
print j, contactpair
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[
self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
# TODO setting jawwidth to 80 is enough
# since fgrpcc already detects inner collisions
tmprtq85.setJawwidth(fgrdist)
tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
axx = tmprtq85.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmprtq85.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(
tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([1, 1, 1, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
# pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
tmprtq85.setColor([.5, 0, 0, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
def __initlftlj(self):
'''
Init the structure of hiro's lft arm links and joints, everything is the same as initrgtlj
author: weiwei
date: 20161107
'''
lftlj = [dict() for i in range(7)]
rngsafemargin = 5
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,145,370.296])
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']
lftlj[1]['name'] = 'link1'
lftlj[1]['mother'] = 0
lftlj[1]['child'] = 2
lftlj[1]['linkpos'] = lftlj[0]['linkend']
lftlj[1]['linkvec'] = np.array([0,95,0])
lftlj[1]['rotax'] = np.array([0,0,1])
lftlj[1]['rotangle'] = 0
lftlj[1]['inherentR'] = rm.rodrigues([1,0,0],-15)
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'] = -(88-rngsafemargin)
lftlj[1]['rngmax'] = +(88-rngsafemargin)
lftlj[2]['name'] = 'link2'
lftlj[2]['mother'] = 1
lftlj[2]['child'] = 3
lftlj[2]['linkpos'] = lftlj[1]['linkend']
lftlj[2]['linkvec'] = np.array([0,0,-250])
lftlj[2]['rotax'] = np.array([0,1,0])
lftlj[2]['rotangle'] = 0
lftlj[2]['rotmat'] = np.dot(lftlj[1]['rotmat'], 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'] = -(140-rngsafemargin)
lftlj[2]['rngmax'] = +(60-rngsafemargin)
lftlj[3]['name'] = 'link3'
lftlj[3]['mother'] = 2
lftlj[3]['child'] = 4
lftlj[3]['linkpos'] = lftlj[2]['linkend']
lftlj[3]['linkvec'] = np.array([-30,0,-235])
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'] = -(158-rngsafemargin)
lftlj[3]['rngmax'] = +(0-rngsafemargin)
lftlj[4]['name'] = 'link4'
lftlj[4]['mother'] = 3
lftlj[4]['child'] = 5
lftlj[4]['linkpos'] = lftlj[3]['linkend']
lftlj[4]['linkvec'] = np.array([0,0,0])
lftlj[4]['rotax'] = np.array([0,0,1])
lftlj[4]['rotangle'] = 0
lftlj[4]['rotmat'] = np.dot(lftlj[3]['rotmat'], 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'] = -(105-rngsafemargin)
lftlj[4]['rngmax'] = +(165-rngsafemargin)
lftlj[5]['name'] = 'link5'
lftlj[5]['mother'] = 4
lftlj[5]['child'] = 6
lftlj[5]['linkpos'] = lftlj[4]['linkend']
lftlj[5]['linkvec'] = np.array([-100,0,-90])
lftlj[5]['rotax'] = np.array([0,1,0])
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'] = -(100-rngsafemargin)
lftlj[5]['rngmax'] = +(100-rngsafemargin)
lftlj[6]['name'] = 'link6'
lftlj[6]['mother'] = 5
lftlj[6]['child'] = -1
lftlj[6]['linkpos'] = lftlj[5]['linkend']
lftlj[6]['linkvec'] = np.array([-145,0,0])
lftlj[6]['rotax'] = np.array([1,0,0])
lftlj[6]['rotangle'] = 0
lftlj[6]['rotmat'] = np.dot(lftlj[5]['rotmat'], 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'] = -(163-rngsafemargin)
lftlj[6]['rngmax'] = +(163-rngsafemargin)
return lftlj
def match(self, perceivedpnts, perceivednormals, ddist = 5.0, dangle = 30.0):
"""
do a match
:return: rotmats of top matches
author: weiwei
date: 20170802
"""
# save txt
pverts = np.array([tuple(x) for x in perceivedpnts], dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text = True).write('perceivedpnts.ply')
# save txt
vandnarray = []
for i in range(len(self.temppnts)):
v = self.temppnts[i]
n = self.tempnormals[i]
vandn = (v[0],v[1],v[2],n[0],n[1],n[2])
vandnarray.append(vandn)
pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\
('nx','f4'),('ny','f4'),('nz','f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text = True).write('ttube.ply')
accspace = {}
# get the preceived global model descriptor
nperceivedpnts = perceivedpnts.shape[0]
i = np.argmax(perceivedpnts, axis = 0)[2]
for j in range(0,nperceivedpnts):
print j, nperceivedpnts
m_0 = np.asarray(perceivedpnts[i])
m_1 = np.asarray(perceivedpnts[j])
v_m0m1 = m_0-m_1
v_m1m0 = m_1-m_0
n_m0 = perceivednormals[i]
n_m1 = perceivednormals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0-m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1/ddist)*ddist+ddist)
f2d = int(math.floor(f2/dangle)*dangle+dangle)
f3d = int(math.floor(f3/dangle)*dangle+dangle)
f4d = int(math.floor(f4/dangle)*dangle+dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1,0,0])
yplus = np.asarray([0,1,0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray([0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2*math.pi - alpha_m0
if key in self.gmd.keys():
malist = self.gmd[key]
print len(malist)
for maslot in malist:
alpha = math.degrees(alpha_m0-maslot[2])
try:
alphadiscrete = int(math.floor(alpha/dangle)*dangle+dangle)
except:
continue
acckey = (maslot[0], alphadiscrete)
if acckey in accspace.keys():
accspace[acckey] += 1
else:
accspace[acckey] = 1
if len(accspace.keys()) is 0:
return (None, None)
# find top matches and rot matrices
maxn = sorted(accspace.iteritems(), key=operator.itemgetter(1), reverse=True)[:5]
rotmat4list = []
silist = []
milist = []
for maxnele in maxn:
mi, alpha = maxnele[0]
# step1 move to temppnts[mi]
displacement0 = -self.temppnts[mi]
rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1], displacement0[2])
# step2 rotate to goal
normalangle = math.degrees(rm.radian_between(self.tempnormals[mi], perceivednormals[i]))
normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i])
normalrotmat = rm.rodrigues(normalrotax, normalangle)
anglerotmat = rm.rodrigues(perceivednormals[i], -alpha)
rotmat = np.dot(anglerotmat, normalrotmat)
rotmat4_1 = pg.npToMat4(rotmat)
# step3 move to perceivedpnts[i]
displacement1 = perceivedpnts[i]
rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1], displacement1[2])
rotmat4 = rotmat4_0*rotmat4_1*rotmat4_2
rotmat4list.append(rotmat4)
silist.append(i)
milist.append(mi)
return rotmat4list, silist, milist
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)
rbd.linearv = rbd.linearv + rbd.dlinearv * dtime
rbd.angularw = rbd.angularw + rbd.dangularw * dtime
return [np.ravel(pl[0:3]), np.ravel(pl[3:6])]
if __name__=="__main__":
import os
import math
from panda3d.core import *
import pandaplotutils.pandageom as pg
import pandaplotutils.pandactrl as pc
base = pc.World(camp = [1000,0,0], lookatp = [0,0,0])
rbd = Rigidbody(mass = 10.0, pos = np.array([0,0,0.0]), com = [0.0,0.0,0.0],
rotmat = rm.rodrigues(np.array([1,0,0]), 1.8),
inertiatensor = np.array([[1732.0,0.0,0.0],[0.0,1732.0,0.0],[0.0,0.0,3393.0]]))
rbd.linearv = np.array([0,0,0])
rbd.angularw = np.array([0,0,50])
this_dir, this_filename = os.path.split(__file__)
model_filepath = Filename.fromOsSpecific(os.path.join(this_dir, "models", "top.egg"))
model = loader.loadModel(model_filepath)
model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
# base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 0])
#
# rbd = Rigidbody(mass = 1.0, pos=np.array([0, 0, 0.0]), rotmat=np.identity(3),
# inertiatensor=np.array([[80833.3, 0.0, 0.0], [0.0, 68333.3, 0.0], [0.0, 0.0, 14166.7]]))
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
tmphand.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter+=1
self.counter = 0
def removeHndccShow(self, base, discretesize=8):
"""
Handcc means hand collision detection
This one is developed for demonstration
This function should be called after executing removeHndcc
:param discretesize: the number of hand orientations
:return: delayTime
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
if self.rtq85plotlist:
for rtq85plotnode in self.rtq85plotlist:
rtq85plotnode.removeNode()
self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
if self.counter2 == 0:
self.counter += 1
if self.counter >= self.facetpairs.shape[0]:
self.counter = 0
self.counter2 += 1
if self.counter2 >= discretesize:
self.counter2 = 0
print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(self.gripcontactpairs_precc[self.counter]):
if j == 0:
print j, contactpair
# for angleid in range(discretesize):
angleid = self.counter2
cctpnt0 = contactpair[0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[self.counter][j][0]
cctnormal1 = [-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]]
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
# TODO setting jawwidth to 80 is enough
# since fgrpcc already detects inner collisions
tmprtq85.setJawwidth(fgrdist)
tmprtq85.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmprtq85.setMat(pandageom.cvtMat4(rotmat) * tmprtq85.getMat())
axx = tmprtq85.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array([axx[0], axx[1], axx[2]])
tmprtq85.setPos(Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
self.hndbullnode = cd.genCollisionMeshMultiNp(tmprtq85.rtq85np, base.render)
result = self.bulletworld.contactTest(self.hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmprtq85.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(self.gripcontactpairnormals_precc[self.counter][j])
# pandageom.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pandageom.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
tmprtq85.setColor([1, 1, 1, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
else:
for contact in result.getContacts():
# cp = contact.getManifoldPoint()
# pandageom.plotSphere(brchild, pos=cp.getLocalPointA(), radius=3, rgba=Vec4(1, 0, 0, 1))
# pandageom.plotSphere(brchild, pos=cp.getLocalPointB(), radius=3, rgba=Vec4(0, 0, 1, 1))
tmprtq85.setColor([.5, 0, 0, .3])
tmprtq85.reparentTo(base.render)
self.rtq85plotlist.append(tmprtq85)
return [np.ravel(pl[0:3]), np.ravel(pl[3:6])]
if __name__ == "__main__":
import os
import math
from panda3d.core import *
import pandaplotutils.pandageom as pg
import pandaplotutils.pandactrl as pc
base = pc.World(camp=[1000, 0, 0], lookatp=[0, 0, 0])
rbd = Rigidbody(mass=10.0,
pos=np.array([0, 0, 0.0]),
com=[0.0, 0.0, 0.0],
rotmat=rm.rodrigues(np.array([1, 0, 0]), 1.8),
inertiatensor=np.array([[1732.0, 0.0, 0.0],
[0.0, 1732.0, 0.0],
[0.0, 0.0, 3393.0]]))
rbd.linearv = np.array([0, 0, 0])
rbd.angularw = np.array([0, 0, 50])
this_dir, this_filename = os.path.split(__file__)
model_filepath = Filename.fromOsSpecific(
os.path.join(this_dir, "models", "top.egg"))
model = loader.loadModel(model_filepath)
model.reparentTo(base.render)
model.setMat(pg.cvtMat4(rbd.rotmat))
model.setPos(pg.npToV3(rbd.pos))
# base = pc.World(camp=[3000, 0, 3000], lookatp=[0, 0, 0])
def computePPFwithAlpha(self, points, normals, ddist = 5.0, dangle = 30.0):
"""
compute the point pair feature f1, f2, f3, f4,
and the alpha_m
dangle in degree
:return: a dictionary
author: weiwei
date: 20170714
"""
gmd = {}
ntemppoint = points.shape[0]
for i in range(ntemppoint):
print i, ntemppoint
for j in range(ntemppoint):
# for i in range(0,1):
# for j in range(3,4):
m_0 = np.asarray(points[i])
m_1 = np.asarray(points[j])
v_m0m1 = m_0-m_1
v_m1m0 = m_1-m_0
n_m0 = normals[i]
n_m1 = normals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0-m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1/ddist)*ddist+ddist)
f2d = int(math.floor(f2/dangle)*dangle+dangle)
f3d = int(math.floor(f3/dangle)*dangle+dangle)
f4d = int(math.floor(f4/dangle)*dangle+dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1,0,0])
yplus = np.asarray([0,1,0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray([0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2*math.pi - alpha_m0
# # debug
# # before transform
# pg.plotArrow(base.render, spos = m_0, epos = m_1, rgba=Vec4(0,1,0,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+n_m0, rgba = Vec4(1,0,0,1))
# # after transform
# # print v_m1m0onxplus
# # print v_m1m0onxplusyzproj
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplus+m_0, rgba=Vec4(0,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplusyzproj+m_0, rgba=Vec4(.70,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus, rgba = Vec4(.7,0,.7,1))
# # alpha_m0
# print np.degrees(alpha_m0)
# # plot aixs
# zplus = np.asarray([0,0,1])
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus*10, rgba = Vec4(.3,0,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+yplus*10, rgba = Vec4(0,.3,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+zplus*10, rgba = Vec4(0,0,.3,.3))
if key in gmd.keys():
gmd[key].append([i, j, alpha_m0])
else:
gmd[key] = [[i, j, alpha_m0]]
for key in gmd.keys():
print key
return gmd
def __initlftlj(self):
'''
Init the structure of hiro's lft arm links and joints, everything is the same as initrgtlj
author: weiwei
date: 20161107
'''
lftlj = [dict() for i in range(7)]
rngsafemargin = 5
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,145,370.296])
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']
lftlj[1]['name'] = 'link1'
lftlj[1]['mother'] = 0
lftlj[1]['child'] = 2
lftlj[1]['linkpos'] = lftlj[0]['linkend']
lftlj[1]['linkvec'] = np.array([0,95,0])
lftlj[1]['rotax'] = np.array([0,0,1])
lftlj[1]['rotangle'] = 0
lftlj[1]['inherentR'] = rm.rodrigues([1,0,0],-15)
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'] = -(88-rngsafemargin)
lftlj[1]['rngmax'] = +(88-rngsafemargin)
lftlj[2]['name'] = 'link2'
lftlj[2]['mother'] = 1
lftlj[2]['child'] = 3
lftlj[2]['linkpos'] = lftlj[1]['linkend']
lftlj[2]['linkvec'] = np.array([0,0,-250])
lftlj[2]['rotax'] = np.array([0,1,0])
lftlj[2]['rotangle'] = 0
lftlj[2]['rotmat'] = np.dot(lftlj[1]['rotmat'], 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'] = -(140-rngsafemargin)
lftlj[2]['rngmax'] = +(60-rngsafemargin)
lftlj[3]['name'] = 'link3'
lftlj[3]['mother'] = 2
lftlj[3]['child'] = 4
lftlj[3]['linkpos'] = lftlj[2]['linkend']
lftlj[3]['linkvec'] = np.array([-30,0,-235])
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'] = -(158-rngsafemargin)
lftlj[3]['rngmax'] = +(0-rngsafemargin)
lftlj[4]['name'] = 'link4'
lftlj[4]['mother'] = 3
lftlj[4]['child'] = 5
lftlj[4]['linkpos'] = lftlj[3]['linkend']
lftlj[4]['linkvec'] = np.array([0,0,0])
lftlj[4]['rotax'] = np.array([0,0,1])
lftlj[4]['rotangle'] = 0
lftlj[4]['rotmat'] = np.dot(lftlj[3]['rotmat'], 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'] = -(105-rngsafemargin)
lftlj[4]['rngmax'] = +(165-rngsafemargin)
lftlj[5]['name'] = 'link5'
lftlj[5]['mother'] = 4
lftlj[5]['child'] = 6
lftlj[5]['linkpos'] = lftlj[4]['linkend']
lftlj[5]['linkvec'] = np.array([-100,0,-90])
lftlj[5]['rotax'] = np.array([0,1,0])
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'] = -(100-rngsafemargin)
lftlj[5]['rngmax'] = +(100-rngsafemargin)
lftlj[6]['name'] = 'link6'
lftlj[6]['mother'] = 5
lftlj[6]['child'] = -1
lftlj[6]['linkpos'] = lftlj[5]['linkend']
lftlj[6]['linkvec'] = np.array([-145,0,0])
lftlj[6]['rotax'] = np.array([1,0,0])
lftlj[6]['rotangle'] = 0
lftlj[6]['rotmat'] = np.dot(lftlj[5]['rotmat'], 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'] = -(163-rngsafemargin)
lftlj[6]['rngmax'] = +(163-rngsafemargin)
return lftlj
def __initrgtlj(self):
'''
Init the structure of hiro's rgt arm links and joints
## output
rgtlj:
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
## note
x is facing forward, y is facing left, z is facing upward
each element of rgtlj is a dictionary
rgtlj[i]['linkpos'] indicates the position of a link
rgtlj[i]['linkvec'] indicates the vector of a link that points from start to end
rgtlj[i]['rotmat'] indicates the frame of this link
rgtlj[i]['rotax'] indicates the rotation axis of the link
rgtlj[i]['rotangle'] indicates the rotation angle of the link around the rotax
rgtlj[i]['linkend'] indicates the end position of the link (passively computed)
## more note:
rgtlj[1]['linkpos'] is the position of the first joint
rgtlj[i]['linkend'] is the same as rgtlj[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
author: weiwei
date: 20160615
'''
# create a arm with six joints
rgtlj = [dict() for i in range(7)]
rngsafemargin = 5
# the 0th link and joint
rgtlj[0]['name'] = 'link0'
rgtlj[0]['mother'] = -1
rgtlj[0]['child'] = 1
rgtlj[0]['linkpos'] = np.array([0,0,0])
rgtlj[0]['linkvec'] = np.array([0,-145,370.296])
rgtlj[0]['rotax'] = np.array([0,0,1])
rgtlj[0]['rotangle'] = 0
rgtlj[0]['rotmat'] = np.eye(3)
rgtlj[0]['linkend'] = np.dot(rgtlj[0]['rotmat'], rgtlj[0]['linkvec'])+rgtlj[0]['linkpos']
# the 1st joint and link
rgtlj[1]['name'] = 'link1'
rgtlj[1]['mother'] = 0
rgtlj[1]['child'] = 2
rgtlj[1]['linkpos'] = rgtlj[0]['linkend']
rgtlj[1]['linkvec'] = np.array([0,-95,0])
rgtlj[1]['rotax'] = np.array([0,0,1])
rgtlj[1]['rotangle'] = 0
rgtlj[1]['inherentR'] = rm.rodrigues([1,0,0], 15)
rgtlj[1]['rotmat'] = np.dot(np.dot(rgtlj[0]['rotmat'], rgtlj[1]['inherentR']), \
rm.rodrigues(rgtlj[1]['rotax'], rgtlj[1]['rotangle']))
rgtlj[1]['linkend'] = np.dot(rgtlj[1]['rotmat'], rgtlj[1]['linkvec'])+rgtlj[1]['linkpos']
rgtlj[1]['rngmin'] = -(88-rngsafemargin)
rgtlj[1]['rngmax'] = +(88-rngsafemargin)
# the 2nd joint and link
rgtlj[2]['name'] = 'link2'
rgtlj[2]['mother'] = 1
rgtlj[2]['child'] = 3
rgtlj[2]['linkpos'] = rgtlj[1]['linkend']
rgtlj[2]['linkvec'] = np.array([0,0,-250])
rgtlj[2]['rotax'] = np.array([0,1,0])
rgtlj[2]['rotangle'] = 0
rgtlj[2]['rotmat'] = np.dot(rgtlj[1]['rotmat'], rm.rodrigues(rgtlj[2]['rotax'], rgtlj[2]['rotangle']))
rgtlj[2]['linkend'] = np.dot(rgtlj[2]['rotmat'], rgtlj[2]['linkvec'])+rgtlj[2]['linkpos']
rgtlj[2]['rngmin'] = -(140-rngsafemargin)
rgtlj[2]['rngmax'] = +(60-rngsafemargin)
# the 3rd joint and link
rgtlj[3]['name'] = 'link3'
rgtlj[3]['mother'] = 2
rgtlj[3]['child'] = 4
rgtlj[3]['linkpos'] = rgtlj[2]['linkend']
rgtlj[3]['linkvec'] = np.array([-30,0,-235])
rgtlj[3]['rotax'] = np.array([0,1,0])
rgtlj[3]['rotangle'] = 0
rgtlj[3]['rotmat'] = np.dot(rgtlj[2]['rotmat'], rm.rodrigues(rgtlj[3]['rotax'], rgtlj[3]['rotangle']))
rgtlj[3]['linkend'] = np.dot(rgtlj[3]['rotmat'], rgtlj[3]['linkvec'])+rgtlj[3]['linkpos']
rgtlj[3]['rngmin'] = -(158-rngsafemargin)
rgtlj[3]['rngmax'] = +(0-rngsafemargin)
# the 4th joint and link
rgtlj[4]['name'] = 'link4'
rgtlj[4]['mother'] = 3
rgtlj[4]['child'] = 5
rgtlj[4]['linkpos'] = rgtlj[3]['linkend']
rgtlj[4]['linkvec'] = np.array([0,0,0])
rgtlj[4]['rotax'] = np.array([0,0,1])
rgtlj[4]['rotangle'] = 0
rgtlj[4]['rotmat'] = np.dot(rgtlj[3]['rotmat'], rm.rodrigues(rgtlj[4]['rotax'], rgtlj[4]['rotangle']))
rgtlj[4]['linkend'] = np.dot(rgtlj[4]['rotmat'], rgtlj[4]['linkvec'])+rgtlj[4]['linkpos']
rgtlj[4]['rngmin'] = -(165-rngsafemargin)
rgtlj[4]['rngmax'] = +(105-rngsafemargin)
# the 5th joint and link
rgtlj[5]['name'] = 'link5'
rgtlj[5]['mother'] = 4
rgtlj[5]['child'] = 6
rgtlj[5]['linkpos'] = rgtlj[4]['linkend']
# the reason 100 is used: -40 is where the ee starts, -60 is where the rqt85 hand locates
rgtlj[5]['linkvec'] = np.array([-100,0,-90])
rgtlj[5]['rotax'] = np.array([0,1,0])
rgtlj[5]['rotangle'] = 0
rgtlj[5]['rotmat'] = np.dot(rgtlj[4]['rotmat'], rm.rodrigues(rgtlj[5]['rotax'], rgtlj[5]['rotangle']))
rgtlj[5]['linkend'] = np.dot(rgtlj[5]['rotmat'], rgtlj[5]['linkvec'])+rgtlj[5]['linkpos']
rgtlj[5]['rngmin'] = -(100-rngsafemargin)
rgtlj[5]['rngmax'] = +(100-rngsafemargin)
# the 6th joint and link
rgtlj[6]['name'] = 'link6'
rgtlj[6]['mother'] = 5
rgtlj[6]['child'] = -1
rgtlj[6]['linkpos'] = rgtlj[5]['linkend']
# the reason 130 is used: the fgrcenter is defined somewhere inside the finger pads
rgtlj[6]['linkvec'] = np.array([-145,0,0])
rgtlj[6]['rotax'] = np.array([1,0,0])
rgtlj[6]['rotangle'] = 0
rgtlj[6]['rotmat'] = np.dot(rgtlj[5]['rotmat'], rm.rodrigues(rgtlj[6]['rotax'], rgtlj[6]['rotangle']))
rgtlj[6]['linkend'] = np.dot(rgtlj[6]['rotmat'], rgtlj[6]['linkvec'])+rgtlj[6]['linkpos']
rgtlj[6]['rngmin'] = -(163-rngsafemargin)
rgtlj[6]['rngmax'] = +(163-rngsafemargin)
return rgtlj
def computePPFwithAlpha(self, points, normals, ddist=5.0, dangle=30.0):
"""
compute the point pair feature f1, f2, f3, f4,
and the alpha_m
dangle in degree
:return: a dictionary
author: weiwei
date: 20170714
"""
gmd = {}
ntemppoint = points.shape[0]
for i in range(ntemppoint):
print i, ntemppoint
for j in range(ntemppoint):
# for i in range(0,1):
# for j in range(3,4):
m_0 = np.asarray(points[i])
m_1 = np.asarray(points[j])
v_m0m1 = m_0 - m_1
v_m1m0 = m_1 - m_0
n_m0 = normals[i]
n_m1 = normals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0 - m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1 / ddist) * ddist + ddist)
f2d = int(math.floor(f2 / dangle) * dangle + dangle)
f3d = int(math.floor(f3 / dangle) * dangle + dangle)
f4d = int(math.floor(f4 / dangle) * dangle + dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1, 0, 0])
yplus = np.asarray([0, 1, 0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray(
[0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2 * math.pi - alpha_m0
# # debug
# # before transform
# pg.plotArrow(base.render, spos = m_0, epos = m_1, rgba=Vec4(0,1,0,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+n_m0, rgba = Vec4(1,0,0,1))
# # after transform
# # print v_m1m0onxplus
# # print v_m1m0onxplusyzproj
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplus+m_0, rgba=Vec4(0,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = v_m1m0onxplusyzproj+m_0, rgba=Vec4(.70,.7,.7,1))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus, rgba = Vec4(.7,0,.7,1))
# # alpha_m0
# print np.degrees(alpha_m0)
# # plot aixs
# zplus = np.asarray([0,0,1])
# pg.plotArrow(base.render, spos = m_0, epos = m_0+xplus*10, rgba = Vec4(.3,0,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+yplus*10, rgba = Vec4(0,.3,0,.3))
# pg.plotArrow(base.render, spos = m_0, epos = m_0+zplus*10, rgba = Vec4(0,0,.3,.3))
if key in gmd.keys():
gmd[key].append([i, j, alpha_m0])
else:
gmd[key] = [[i, j, alpha_m0]]
for key in gmd.keys():
print key
return gmd
def __initrgtlj(self):
"""
Init rgt arm links and joints
## note
x is facing forward, y is facing left, z is facing upward
each element of rgtlj is a dictionary
rgtlj[i]['linkpos'] indicates the position of a link
rgtlj[i]['linkvec'] indicates the vector of a link that points from start to end
rgtlj[i]['rotmat'] indicates the frame of this link
rgtlj[i]['rotax'] indicates the rotation axis of the link
rgtlj[i]['rotangle'] indicates the rotation angle of the link around the rotax
rgtlj[i]['linkend'] indicates the end position of the link (passively computed)
## more note:
rgtlj[1]['linkpos'] is the position of the first joint
rgtlj[i]['linkend'] is the same as rgtlj[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:
rgtlj:
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: 20170626, tsukuba
"""
# create a arm with 9 joints
rgtlj = [dict() for i in range(8)]
rngsafemargin = 5
# the 0th link and joint
rgtlj[0]['name'] = 'link0'
rgtlj[0]['mother'] = -1
rgtlj[0]['child'] = 1
rgtlj[0]['linkpos'] = np.array([0.0, 0.0, 0.0])
rgtlj[0]['linkvec'] = np.array([8.0, -250.0, 181.0])
rgtlj[0]['rotax'] = np.array([0, 0, 1])
rgtlj[0]['rotangle'] = 0
rgtlj[0]['rotmat'] = np.eye(3)
rgtlj[0]['linkend'] = np.dot(rgtlj[0]['rotmat'],
rgtlj[0]['linkvec']) + rgtlj[0]['linkpos']
# the 1st joint and link
rgtlj[1]['name'] = 'link1'
rgtlj[1]['mother'] = 0
rgtlj[1]['child'] = 2
rgtlj[1]['linkpos'] = rgtlj[0]['linkend']
rgtlj[1]['linkvec'] = np.array([0.0, 0.0, 0.0])
rgtlj[1]['rotax'] = np.array([0, 1, 0])
rgtlj[1]['rotangle'] = 0
rgtlj[1]['rotmat'] = np.dot(
rgtlj[0]['rotmat'],
rm.rodrigues(rgtlj[1]['rotax'], rgtlj[1]['rotangle']))
rgtlj[1]['linkend'] = np.dot(rgtlj[1]['rotmat'],
rgtlj[1]['linkvec']) + rgtlj[1]['linkpos']
rgtlj[1]['rngmin'] = -(180 - rngsafemargin)
rgtlj[1]['rngmax'] = +(60 - rngsafemargin)
# the 2nd joint and link
rgtlj[2]['name'] = 'link2'
rgtlj[2]['mother'] = 1
rgtlj[2]['child'] = 3
rgtlj[2]['linkpos'] = rgtlj[1]['linkend']
rgtlj[2]['linkvec'] = np.array([0.0, 0.0, 0.0])
rgtlj[2]['rotax'] = np.array([1, 0, 0])
rgtlj[2]['rotangle'] = 0
rgtlj[2]['rotmat'] = np.dot(
rgtlj[1]['rotmat'],
rm.rodrigues(rgtlj[2]['rotax'], rgtlj[2]['rotangle']))
rgtlj[2]['linkend'] = np.dot(rgtlj[2]['rotmat'],
rgtlj[2]['linkvec']) + rgtlj[2]['linkpos']
rgtlj[2]['rngmin'] = -(10 - rngsafemargin)
rgtlj[2]['rngmax'] = +(90 - rngsafemargin)
# the 3rd joint and link
rgtlj[3]['name'] = 'link3'
rgtlj[3]['mother'] = 2
rgtlj[3]['child'] = 4
rgtlj[3]['linkpos'] = rgtlj[2]['linkend']
rgtlj[3]['linkvec'] = np.array([0.0, 0.0, -300.0])
rgtlj[3]['rotax'] = np.array([0, 0, 1])
rgtlj[3]['rotangle'] = 0
rgtlj[3]['rotmat'] = np.dot(
rgtlj[2]['rotmat'],
rm.rodrigues(rgtlj[3]['rotax'], rgtlj[3]['rotangle']))
rgtlj[3]['linkend'] = np.dot(rgtlj[3]['rotmat'],
rgtlj[3]['linkvec']) + rgtlj[3]['linkpos']
rgtlj[3]['rngmin'] = -(92 - rngsafemargin)
rgtlj[3]['rngmax'] = +(92 - rngsafemargin)
# the 4th joint and link
rgtlj[4]['name'] = 'link4'
rgtlj[4]['mother'] = 3
rgtlj[4]['child'] = 5
rgtlj[4]['linkpos'] = rgtlj[3]['linkend']
rgtlj[4]['linkvec'] = np.array([0.0, 0.0, -300.0])
rgtlj[4]['rotax'] = np.array([0, 1, 0])
rgtlj[4]['rotangle'] = 0
rgtlj[4]['rotmat'] = np.dot(
rgtlj[3]['rotmat'],
rm.rodrigues(rgtlj[4]['rotax'], rgtlj[4]['rotangle']))
rgtlj[4]['linkend'] = np.dot(rgtlj[4]['rotmat'],
rgtlj[4]['linkvec']) + rgtlj[4]['linkpos']
rgtlj[4]['rngmin'] = -(137 - rngsafemargin)
rgtlj[4]['rngmax'] = +(2 - rngsafemargin)
# the 5th joint and link
rgtlj[5]['name'] = 'link5'
rgtlj[5]['mother'] = 4
rgtlj[5]['child'] = 6
rgtlj[5]['linkpos'] = rgtlj[4]['linkend']
rgtlj[5]['linkvec'] = np.array([0.0, 0.0, 0.0])
rgtlj[5]['rotax'] = np.array([0, 0, 1])
rgtlj[5]['rotangle'] = 0
rgtlj[5]['rotmat'] = np.dot(
rgtlj[4]['rotmat'],
rm.rodrigues(rgtlj[5]['rotax'], rgtlj[5]['rotangle']))
rgtlj[5]['linkend'] = np.dot(rgtlj[5]['rotmat'],
rgtlj[5]['linkvec']) + rgtlj[5]['linkpos']
rgtlj[5]['rngmin'] = -(106 - rngsafemargin)
rgtlj[5]['rngmax'] = +(106 - rngsafemargin)
# the 6th joint and link
rgtlj[6]['name'] = 'link6'
rgtlj[6]['mother'] = 5
rgtlj[6]['child'] = 7
rgtlj[6]['linkpos'] = rgtlj[5]['linkend']
rgtlj[6]['linkvec'] = np.array([0.0, 0.0, 0.0])
rgtlj[6]['rotax'] = np.array([1, 0, 0])
rgtlj[6]['rotangle'] = 0
rgtlj[6]['rotmat'] = np.dot(
rgtlj[5]['rotmat'],
rm.rodrigues(rgtlj[6]['rotax'], rgtlj[6]['rotangle']))
rgtlj[6]['linkend'] = np.dot(rgtlj[6]['rotmat'],
rgtlj[6]['linkvec']) + rgtlj[6]['linkpos']
rgtlj[6]['rngmin'] = -(112 - rngsafemargin)
rgtlj[6]['rngmax'] = +(112 - rngsafemargin)
# the 7th joint and link
rgtlj[7]['name'] = 'link7'
rgtlj[7]['mother'] = 6
rgtlj[7]['child'] = -1
rgtlj[7]['linkpos'] = rgtlj[6]['linkend']
rgtlj[7]['linkvec'] = np.array([-200.0, 0.0, 0.0])
rgtlj[7]['rotax'] = np.array([1, 0, 0])
rgtlj[7]['rotangle'] = 0
rgtlj[7]['inherentR'] = rm.rodrigues([0, 1, 0], -90)
rgtlj[7]['rotmat'] = np.dot(np.dot(rgtlj[6]['rotmat'], rgtlj[7]['inherentR']), \
rm.rodrigues(rgtlj[7]['rotax'], rgtlj[7]['rotangle']))
rgtlj[7]['linkend'] = np.dot(rgtlj[7]['rotmat'],
rgtlj[7]['linkvec']) + rgtlj[7]['linkpos']
rgtlj[7]['rngmin'] = -(152 - rngsafemargin)
rgtlj[7]['rngmax'] = +(152 - rngsafemargin)
return rgtlj
def saveToDB(self, positionlist, gdb, discretesize=4):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "(" + str(360 * i * 1.0 / discretesize) + "), "
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idstartgoal FROM startgoal,freetabletopplacement,object WHERE \
startgoal.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO startgoal(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist,
anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2] + ";"
gdb.execute(sql)
else:
print "startgoal already exist!"
print "Save to DB done!"
starttreesamplerate = 25
endtreesamplerate = 30
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, -56.0889005661, 1150.5])
startrot = np.array([[1, 0, 0], [0, -0.92388, -0.382683],
[0, 0.382683, -0.92388]]).T
startrot = np.dot(rm.rodrigues([1, 0, 0], 20), startrot)
startpos = startpos - 70.0 * startrot[:, 2]
goalpos = np.array([354.5617667638, -500, 1050.5])
goalrot = np.dot(rm.rodrigues([1, 0, 0], -90), startrot)
goalpos = goalpos - 150.0 * goalrot[:, 2]
# goalpos2 = goalpos2 - 150.0*goalrot2[:,2]
startjnts = robot.numikr(startpos, startrot, armname=armname)
print(startjnts)
goaljnts = robot.numikr(goalpos, goalrot, armname=armname)
print(goaljnts)
base.pggen.plotAxis(base.render,
spos=startpos,
pandamat3=base.pg.npToMat3(startrot))
base.pggen.plotAxis(base.render,
spos=goalpos,
pandamat3=base.pg.npToMat3(goalrot))
[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, expanddis=5,
maxiter=2000, maxtime=100.0)
robot.movearmfk(goal3, armname)
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 six joints
lftlj = [dict() for i in range(10)]
rngsafemargin = 5
# 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([52, 80, 533])
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, 130, 0])
lftlj[1]['rotax'] = np.array([0, 0, 1])
lftlj[1]['rotangle'] = 0
lftlj[1]['rotmat'] = np.dot(
lftlj[0]['rotmat'],
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'] = -(60 - rngsafemargin)
lftlj[1]['rngmax'] = +(20 - 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, 0, 0])
lftlj[2]['rotax'] = np.array([0, 1, 0])
lftlj[2]['rotangle'] = 0
lftlj[2]['rotmat'] = np.dot(
lftlj[1]['rotmat'],
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'] = -(200 - rngsafemargin)
lftlj[2]['rngmax'] = +(70 - 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, 20, 0])
lftlj[3]['rotax'] = np.array([1, 0, 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'] = -(25 - rngsafemargin)
lftlj[3]['rngmax'] = +(95 - 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([50, 0, -295.804])
lftlj[4]['rotax'] = np.array([0, 0, 1])
lftlj[4]['rotangle'] = 0
lftlj[4]['rotmat'] = np.dot(
lftlj[3]['rotmat'],
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'] = -(120 - rngsafemargin)
lftlj[4]['rngmax'] = +(92 - 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([-50, 0, -295.804])
lftlj[5]['rotax'] = np.array([0, 1, 0])
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'] = -(180 - rngsafemargin)
lftlj[5]['rngmax'] = -(17.2 - rngsafemargin)
# the 6th joint and link
lftlj[6]['name'] = 'link6'
lftlj[6]['mother'] = 5
lftlj[6]['child'] = 7
lftlj[6]['linkpos'] = lftlj[5]['linkend']
lftlj[6]['linkvec'] = np.array([0, 0, 0])
lftlj[6]['rotax'] = np.array([0, 0, 1])
lftlj[6]['rotangle'] = 0
lftlj[6]['rotmat'] = np.dot(
lftlj[5]['rotmat'],
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'] = -(182 - rngsafemargin)
lftlj[6]['rngmax'] = +(122 - rngsafemargin)
# the 7th joint and link
lftlj[7]['name'] = 'link7'
lftlj[7]['mother'] = 6
lftlj[7]['child'] = 8
lftlj[7]['linkpos'] = lftlj[6]['linkend']
lftlj[7]['linkvec'] = np.array([0, 0, 0])
lftlj[7]['rotax'] = np.array([1, 0, 0])
lftlj[7]['rotangle'] = 0
lftlj[7]['inherentR'] = rm.rodrigues([1, 0, 0], -45)
lftlj[7]['rotmat'] = np.dot(np.dot(lftlj[6]['rotmat'], lftlj[7]['inherentR']), \
rm.rodrigues(lftlj[7]['rotax'], lftlj[7]['rotangle']))
lftlj[7]['linkend'] = np.dot(lftlj[7]['rotmat'],
lftlj[7]['linkvec']) + lftlj[7]['linkpos']
lftlj[7]['rngmin'] = -(70 - rngsafemargin)
lftlj[7]['rngmax'] = +(47 - rngsafemargin)
# the 8th joint and link
lftlj[8]['name'] = 'link8'
lftlj[8]['mother'] = 7
lftlj[8]['child'] = 9
lftlj[8]['linkpos'] = lftlj[7]['linkend']
lftlj[8]['linkvec'] = np.array([-55, 0, 37])
lftlj[8]['rotax'] = np.array([0.707, 0, 0.707])
lftlj[8]['rotangle'] = 0
# make sure x direction faces at ee, z directions faces downward
# see the definition of the coordinates in rtq85 (execute the file)
lftlj[8]['inherentR'] = np.dot(rm.rodrigues([1, 0, 0], 45),
rm.rodrigues([0, 1, 0], 180))
lftlj[8]['inherentR'] = np.dot(lftlj[8]['inherentR'],
rm.rodrigues([0, 0, 1], 90))
lftlj[8]['rotmat'] = np.dot(np.dot(lftlj[7]['rotmat'], lftlj[8]['inherentR']), \
rm.rodrigues(lftlj[8]['rotax'], lftlj[8]['rotangle']))
lftlj[8]['linkend'] = np.dot(lftlj[8]['rotmat'],
lftlj[8]['linkvec']) + lftlj[8]['linkpos']
lftlj[8]['rngmin'] = -(42 - rngsafemargin)
lftlj[8]['rngmax'] = +(42 - rngsafemargin)
# the 9th joint and link
lftlj[9]['name'] = 'link9'
lftlj[9]['mother'] = 8
lftlj[9]['child'] = -1
lftlj[9]['linkpos'] = lftlj[8]['linkend']
lftlj[9]['linkvec'] = np.array([-130, 0, 0])
lftlj[9]['rotax'] = np.array([1, 0, 0])
lftlj[9]['rotangle'] = 0
lftlj[9]['rotmat'] = np.dot(
lftlj[8]['rotmat'],
rm.rodrigues(lftlj[9]['rotax'], lftlj[9]['rotangle']))
lftlj[9]['linkend'] = np.dot(lftlj[9]['rotmat'],
lftlj[9]['linkvec']) + lftlj[9]['linkpos']
lftlj[9]['rngmin'] = -(300 - rngsafemargin)
lftlj[9]['rngmax'] = +(300 - rngsafemargin)
return lftlj
import utils.robotmath as rm
import numpy as np
from panda3d.core import *
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.run()
def __initrgtlj(self):
'''
Init the structure of hiro's rgt arm links and joints
## output
rgtlj:
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
## note
x is facing forward, y is facing left, z is facing upward
each element of rgtlj is a dictionary
rgtlj[i]['linkpos'] indicates the position of a link
rgtlj[i]['linkvec'] indicates the vector of a link that points from start to end
rgtlj[i]['rotmat'] indicates the frame of this link
rgtlj[i]['rotax'] indicates the rotation axis of the link
rgtlj[i]['rotangle'] indicates the rotation angle of the link around the rotax
rgtlj[i]['linkend'] indicates the end position of the link (passively computed)
## more note:
rgtlj[1]['linkpos'] is the position of the first joint
rgtlj[i]['linkend'] is the same as rgtlj[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
author: weiwei
date: 20160615
'''
# create a arm with six joints
rgtlj = [dict() for i in range(7)]
rngsafemargin = 5
# the 0th link and joint
rgtlj[0]['name'] = 'link0'
rgtlj[0]['mother'] = -1
rgtlj[0]['child'] = 1
rgtlj[0]['linkpos'] = np.array([0,0,0])
rgtlj[0]['linkvec'] = np.array([0,-145,370.296])
rgtlj[0]['rotax'] = np.array([0,0,1])
rgtlj[0]['rotangle'] = 0
rgtlj[0]['rotmat'] = np.eye(3)
rgtlj[0]['linkend'] = np.dot(rgtlj[0]['rotmat'], rgtlj[0]['linkvec'])+rgtlj[0]['linkpos']
# the 1st joint and link
rgtlj[1]['name'] = 'link1'
rgtlj[1]['mother'] = 0
rgtlj[1]['child'] = 2
rgtlj[1]['linkpos'] = rgtlj[0]['linkend']
rgtlj[1]['linkvec'] = np.array([0,-95,0])
rgtlj[1]['rotax'] = np.array([0,0,1])
rgtlj[1]['rotangle'] = 0
rgtlj[1]['inherentR'] = rm.rodrigues([1,0,0], 15)
rgtlj[1]['rotmat'] = np.dot(np.dot(rgtlj[0]['rotmat'], rgtlj[1]['inherentR']), \
rm.rodrigues(rgtlj[1]['rotax'], rgtlj[1]['rotangle']))
rgtlj[1]['linkend'] = np.dot(rgtlj[1]['rotmat'], rgtlj[1]['linkvec'])+rgtlj[1]['linkpos']
rgtlj[1]['rngmin'] = -(88-rngsafemargin)
rgtlj[1]['rngmax'] = +(88-rngsafemargin)
# the 2nd joint and link
rgtlj[2]['name'] = 'link2'
rgtlj[2]['mother'] = 1
rgtlj[2]['child'] = 3
rgtlj[2]['linkpos'] = rgtlj[1]['linkend']
rgtlj[2]['linkvec'] = np.array([0,0,-250])
rgtlj[2]['rotax'] = np.array([0,1,0])
rgtlj[2]['rotangle'] = 0
rgtlj[2]['rotmat'] = np.dot(rgtlj[1]['rotmat'], rm.rodrigues(rgtlj[2]['rotax'], rgtlj[2]['rotangle']))
rgtlj[2]['linkend'] = np.dot(rgtlj[2]['rotmat'], rgtlj[2]['linkvec'])+rgtlj[2]['linkpos']
rgtlj[2]['rngmin'] = -(140-rngsafemargin)
rgtlj[2]['rngmax'] = +(60-rngsafemargin)
# the 3rd joint and link
rgtlj[3]['name'] = 'link3'
rgtlj[3]['mother'] = 2
rgtlj[3]['child'] = 4
rgtlj[3]['linkpos'] = rgtlj[2]['linkend']
rgtlj[3]['linkvec'] = np.array([-30,0,-235])
rgtlj[3]['rotax'] = np.array([0,1,0])
rgtlj[3]['rotangle'] = 0
rgtlj[3]['rotmat'] = np.dot(rgtlj[2]['rotmat'], rm.rodrigues(rgtlj[3]['rotax'], rgtlj[3]['rotangle']))
rgtlj[3]['linkend'] = np.dot(rgtlj[3]['rotmat'], rgtlj[3]['linkvec'])+rgtlj[3]['linkpos']
rgtlj[3]['rngmin'] = -(158-rngsafemargin)
rgtlj[3]['rngmax'] = +(0-rngsafemargin)
# the 4th joint and link
rgtlj[4]['name'] = 'link4'
rgtlj[4]['mother'] = 3
rgtlj[4]['child'] = 5
rgtlj[4]['linkpos'] = rgtlj[3]['linkend']
rgtlj[4]['linkvec'] = np.array([0,0,0])
rgtlj[4]['rotax'] = np.array([0,0,1])
rgtlj[4]['rotangle'] = 0
rgtlj[4]['rotmat'] = np.dot(rgtlj[3]['rotmat'], rm.rodrigues(rgtlj[4]['rotax'], rgtlj[4]['rotangle']))
rgtlj[4]['linkend'] = np.dot(rgtlj[4]['rotmat'], rgtlj[4]['linkvec'])+rgtlj[4]['linkpos']
rgtlj[4]['rngmin'] = -(165-rngsafemargin)
rgtlj[4]['rngmax'] = +(105-rngsafemargin)
# the 5th joint and link
rgtlj[5]['name'] = 'link5'
rgtlj[5]['mother'] = 4
rgtlj[5]['child'] = 6
rgtlj[5]['linkpos'] = rgtlj[4]['linkend']
# the reason 100 is used: -40 is where the ee starts, -60 is where the rqt85 hand locates
rgtlj[5]['linkvec'] = np.array([-100,0,-90])
rgtlj[5]['rotax'] = np.array([0,1,0])
rgtlj[5]['rotangle'] = 0
rgtlj[5]['rotmat'] = np.dot(rgtlj[4]['rotmat'], rm.rodrigues(rgtlj[5]['rotax'], rgtlj[5]['rotangle']))
rgtlj[5]['linkend'] = np.dot(rgtlj[5]['rotmat'], rgtlj[5]['linkvec'])+rgtlj[5]['linkpos']
rgtlj[5]['rngmin'] = -(100-rngsafemargin)
rgtlj[5]['rngmax'] = +(100-rngsafemargin)
# the 6th joint and link
rgtlj[6]['name'] = 'link6'
rgtlj[6]['mother'] = 5
rgtlj[6]['child'] = -1
rgtlj[6]['linkpos'] = rgtlj[5]['linkend']
# the reason 130 is used: the fgrcenter is defined somewhere inside the finger pads
rgtlj[6]['linkvec'] = np.array([-145,0,0])
rgtlj[6]['rotax'] = np.array([1,0,0])
rgtlj[6]['rotangle'] = 0
rgtlj[6]['rotmat'] = np.dot(rgtlj[5]['rotmat'], rm.rodrigues(rgtlj[6]['rotax'], rgtlj[6]['rotangle']))
rgtlj[6]['linkend'] = np.dot(rgtlj[6]['rotmat'], rgtlj[6]['linkvec'])+rgtlj[6]['linkpos']
rgtlj[6]['rngmin'] = -(163-rngsafemargin)
rgtlj[6]['rngmax'] = +(163-rngsafemargin)
return rgtlj
def saveToDB(self, positionlist, gdb, discretesize=8.0):
"""
:param positionlist: a list of positions to place the object one the table
:param discretesize: the discretization of rotation angles around z axis
:return:
author: weiwei
date: 20161215, osaka
"""
# save discretiezed angle
sql = "SELECT * FROM angle"
result = gdb.execute(sql)
if len(result) == 0:
sql = "INSERT INTO angle(value) VALUES "
for i in range(discretesize):
sql += "("+str(360*i*1.0/discretesize)+"), "
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print "Angles already set!"
# save tabletopplacements
sql = "SELECT idtabletopplacements FROM tabletopplacements,freetabletopplacement,object WHERE \
tabletopplacements.idfreetabletopplacement=freetabletopplacement.idfreetabletopplacement AND \
freetabletopplacement.idobject=object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
# 1) select the freetabletopplacement
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
result = np.asarray(result)
idfreelist = [int(x) for x in result[:, 0]]
rotmatfreelist = [dc.strToMat4(x) for x in result[:, 1]]
# 2) select the angle
sql = "SELECT angle.idangle,angle.value FROM angle"
result = np.asarray(gdb.execute(sql))
idanglelist = [int(x) for x in result[:, 0]]
anglevaluelist = [float(x) for x in result[:, 1]]
# 3) save to database
sql = "INSERT INTO tabletopplacements(rotmat, tabletopposition, idangle, idfreetabletopplacement) VALUES "
for ttoppos in positionlist:
ttoppos = Point3(ttoppos[0], ttoppos[1], ttoppos[2])
for idfree, rotmatfree in zip(idfreelist, rotmatfreelist):
for idangle, anglevalue in zip(idanglelist, anglevaluelist):
rotangle = anglevalue
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
varrotmat = rotmatfree * rotmat4
sql += "('%s', '%s', %d, %d), " % \
(dc.mat4ToStr(varrotmat), dc.v3ToStr(ttoppos), idangle, idfree)
sql = sql[:-2]+";"
gdb.execute(sql)
else:
print "Tabletopplacements already exist!"
# save tabletopgrips
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT tabletopgrips.idtabletopgrips FROM tabletopgrips,freeairgrip,object WHERE \
tabletopgrips.idfreeairgrip=freeairgrip.idfreeairgrip AND \
freeairgrip.idobject=object.idobject AND object.name LIKE '%s' AND \
freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = gdb.execute(sql)
if len(result) == 0:
sql = "SELECT freetabletopplacement.idfreetabletopplacement \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s'" % self.dbobjname
result = gdb.execute(sql)
if len(result) == 0:
raise ValueError("Plan the freetabletopplacement first!")
for idfree in result:
idfree = int(idfree[0])
sql = "SELECT tabletopplacements.idtabletopplacements, \
tabletopplacements.tabletopposition, angle.value,\
freetabletopgrip.contactpoint0, freetabletopgrip.contactpoint1, \
freetabletopgrip.contactnormal0, freetabletopgrip.contactnormal1, \
freetabletopgrip.rotmat, freetabletopgrip.jawwidth, freetabletopgrip.idfreeairgrip \
FROM tabletopplacements,freetabletopplacement,freetabletopgrip,freeairgrip,angle WHERE \
tabletopplacements.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
tabletopplacements.idangle = angle.idangle AND \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freeairgrip.idhand = %d AND \
freetabletopplacement.idfreetabletopplacement = %d" % (idhand, idfree)
result1 = gdb.execute(sql)
if len(result1) == 0:
# no grasp availalbe?
continue
# sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, \
# contactnormal1, rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES "
if len(result1) > 20000:
result1 = result1[0::int(len(result1)/20000.0)]
result1 = np.asarray(result1)
idtabletopplacementslist = [int(x) for x in result1[:,0]]
tabletoppositionlist = [dc.strToV3(x) for x in result1[:,1]]
rotanglelist = [float(x) for x in result1[:,2]]
freegripcontactpoint0list = [dc.strToV3(x) for x in result1[:,3]]
freegripcontactpoint1list = [dc.strToV3(x) for x in result1[:,4]]
freegripcontactnormal0list = [dc.strToV3(x) for x in result1[:,5]]
freegripcontactnormal1list = [dc.strToV3(x) for x in result1[:,6]]
freegriprotmatlist = [dc.strToMat4(x) for x in result1[:,7]]
freegripjawwidthlist = [float(x) for x in result1[:,8]]
freegripidlist = [int(x) for x in result1[:,9]]
for idtabletopplacements, ttoppos, rotangle, cct0, cct1, cctn0, cctn1, \
freegriprotmat, jawwidth, idfreegrip in zip(idtabletopplacementslist, \
tabletoppositionlist, rotanglelist, freegripcontactpoint0list, freegripcontactpoint1list, \
freegripcontactnormal0list, freegripcontactnormal1list, freegriprotmatlist, freegripjawwidthlist, \
freegripidlist):
rotmat = rm.rodrigues([0, 0, 1], rotangle)
# rotmat4 = Mat4(rotmat[0][0], rotmat[0][1], rotmat[0][2], 0,
# rotmat[1][0], rotmat[1][1], rotmat[1][2], 0,
# rotmat[2][0], rotmat[2][1], rotmat[2][2], 0,
# ttoppos[0], ttoppos[1], ttoppos[2], 1)
rotmat4 = pg.cvtMat4(rotmat, ttoppos)
ttpcct0 = rotmat4.xformPoint(cct0)
ttpcct1 = rotmat4.xformPoint(cct1)
ttpcctn0 = rotmat4.xformVec(cctn0)
ttpcctn1 = rotmat4.xformVec(cctn1)
ttpgriprotmat = freegriprotmat*rotmat4
sql = "INSERT INTO tabletopgrips(contactpnt0, contactpnt1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreeairgrip, idtabletopplacements) VALUES \
('%s', '%s', '%s', '%s', '%s', '%s', %d, %d) " % \
(dc.v3ToStr(ttpcct0), dc.v3ToStr(ttpcct1), dc.v3ToStr(ttpcctn0), dc.v3ToStr(ttpcctn1), \
dc.mat4ToStr(ttpgriprotmat), str(jawwidth), idfreegrip, idtabletopplacements)
gdb.execute(sql)
else:
print "Tabletopgrips already exist!"
print "Save to DB done!"
def __init__(self, jawwidth=85, hndcolor=None):
'''
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
## output
rtq85np:
the nodepath of this rtq85 hand
author: weiwei
date: 20160627
'''
self.rtq85np = NodePath("rtq85hnd")
self.handnp = self.rtq85np
self.jawwidth = jawwidth
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)
rtq85base.setPos(0, 0, 0)
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.cvtMat4(rm.rodrigues([0, 0, 1], 90)) *
rtq85base.getMat())
rtq85base.reparentTo(self.rtq85np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 85.0
self.__jawwidthclosed = 0.0
def __initrgtlj(self):
"""
Init rgt arm links and joints
## note
x is facing forward, y is facing left, z is facing upward
each element of rgtlj is a dictionary
rgtlj[i]['linkpos'] indicates the position of a link
rgtlj[i]['linkvec'] indicates the vector of a link that points from start to end
rgtlj[i]['rotmat'] indicates the frame of this link
rgtlj[i]['rotax'] indicates the rotation axis of the link
rgtlj[i]['rotangle'] indicates the rotation angle of the link around the rotax
rgtlj[i]['linkend'] indicates the end position of the link (passively computed)
## more note:
rgtlj[1]['linkpos'] is the position of the first joint
rgtlj[i]['linkend'] is the same as rgtlj[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:
rgtlj:
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 10 joints
rgtlj = [dict() for i in range(10)]
rngsafemargin = 5
# the 0th link and joint
rgtlj[0]['name'] = 'link0'
rgtlj[0]['mother'] = -1
rgtlj[0]['child'] = 1
rgtlj[0]['linkpos'] = np.array([0,0,0])
rgtlj[0]['linkvec'] = np.array([32.0, -80.0, 542.0])
rgtlj[0]['rotax'] = np.array([0,0,1])
rgtlj[0]['rotangle'] = 0
rgtlj[0]['rotmat'] = np.eye(3)
rgtlj[0]['linkend'] = np.dot(rgtlj[0]['rotmat'], rgtlj[0]['linkvec'])+rgtlj[0]['linkpos']
# the 1st joint and link
rgtlj[1]['name'] = 'link1'
rgtlj[1]['mother'] = 0
rgtlj[1]['child'] = 2
rgtlj[1]['linkpos'] = rgtlj[0]['linkend']
rgtlj[1]['linkvec'] = np.array([0,-130,0])
rgtlj[1]['rotax'] = np.array([0,0,1])
rgtlj[1]['rotangle'] = 0
rgtlj[1]['rotmat'] = np.dot(rgtlj[0]['rotmat'], rm.rodrigues(rgtlj[1]['rotax'], rgtlj[1]['rotangle']))
rgtlj[1]['linkend'] = np.dot(rgtlj[1]['rotmat'], rgtlj[1]['linkvec'])+rgtlj[1]['linkpos']
rgtlj[1]['rngmin'] = -(20-rngsafemargin)
rgtlj[1]['rngmax'] = +(60-rngsafemargin)
# the 2nd joint and link
rgtlj[2]['name'] = 'link2'
rgtlj[2]['mother'] = 1
rgtlj[2]['child'] = 3
rgtlj[2]['linkpos'] = rgtlj[1]['linkend']
rgtlj[2]['linkvec'] = np.array([0,0,0])
rgtlj[2]['rotax'] = np.array([0,1,0])
rgtlj[2]['rotangle'] = 0
rgtlj[2]['rotmat'] = np.dot(rgtlj[1]['rotmat'], rm.rodrigues(rgtlj[2]['rotax'], rgtlj[2]['rotangle']))
rgtlj[2]['linkend'] = np.dot(rgtlj[2]['rotmat'], rgtlj[2]['linkvec'])+rgtlj[2]['linkpos']
rgtlj[2]['rngmin'] = -(200-rngsafemargin)
rgtlj[2]['rngmax'] = +(70-rngsafemargin)
# the 3rd joint and link
rgtlj[3]['name'] = 'link3'
rgtlj[3]['mother'] = 2
rgtlj[3]['child'] = 4
rgtlj[3]['linkpos'] = rgtlj[2]['linkend']
rgtlj[3]['linkvec'] = np.array([0,-20,0])
rgtlj[3]['rotax'] = np.array([1,0,0])
rgtlj[3]['rotangle'] = 0
rgtlj[3]['rotmat'] = np.dot(rgtlj[2]['rotmat'], rm.rodrigues(rgtlj[3]['rotax'], rgtlj[3]['rotangle']))
rgtlj[3]['linkend'] = np.dot(rgtlj[3]['rotmat'], rgtlj[3]['linkvec'])+rgtlj[3]['linkpos']
rgtlj[3]['rngmin'] = -(95-rngsafemargin)
rgtlj[3]['rngmax'] = +(25-rngsafemargin)
# the 4th joint and link
rgtlj[4]['name'] = 'link4'
rgtlj[4]['mother'] = 3
rgtlj[4]['child'] = 5
rgtlj[4]['linkpos'] = rgtlj[3]['linkend']
rgtlj[4]['linkvec'] = np.array([50,0,-295.804])
rgtlj[4]['rotax'] = np.array([0,0,1])
rgtlj[4]['rotangle'] = 0
rgtlj[4]['rotmat'] = np.dot(rgtlj[3]['rotmat'], rm.rodrigues(rgtlj[4]['rotax'], rgtlj[4]['rotangle']))
rgtlj[4]['linkend'] = np.dot(rgtlj[4]['rotmat'], rgtlj[4]['linkvec'])+rgtlj[4]['linkpos']
rgtlj[4]['rngmin'] = -(92-rngsafemargin)
rgtlj[4]['rngmax'] = +(120-rngsafemargin)
# the 5th joint and link
rgtlj[5]['name'] = 'link5'
rgtlj[5]['mother'] = 4
rgtlj[5]['child'] = 6
rgtlj[5]['linkpos'] = rgtlj[4]['linkend']
rgtlj[5]['linkvec'] = np.array([-50,0,-295.804])
rgtlj[5]['rotax'] = np.array([0,1,0])
rgtlj[5]['rotangle'] = 0
rgtlj[5]['rotmat'] = np.dot(rgtlj[4]['rotmat'], rm.rodrigues(rgtlj[5]['rotax'], rgtlj[5]['rotangle']))
rgtlj[5]['linkend'] = np.dot(rgtlj[5]['rotmat'], rgtlj[5]['linkvec'])+rgtlj[5]['linkpos']
rgtlj[5]['rngmin'] = -(180-rngsafemargin)
rgtlj[5]['rngmax'] = -(17.2-rngsafemargin)
# the 6th joint and link
rgtlj[6]['name'] = 'link6'
rgtlj[6]['mother'] = 5
rgtlj[6]['child'] = 7
rgtlj[6]['linkpos'] = rgtlj[5]['linkend']
rgtlj[6]['linkvec'] = np.array([0,0,0])
rgtlj[6]['rotax'] = np.array([0,0,1])
rgtlj[6]['rotangle'] = 0
rgtlj[6]['rotmat'] = np.dot(rgtlj[5]['rotmat'], rm.rodrigues(rgtlj[6]['rotax'], rgtlj[6]['rotangle']))
rgtlj[6]['linkend'] = np.dot(rgtlj[6]['rotmat'], rgtlj[6]['linkvec'])+rgtlj[6]['linkpos']
rgtlj[6]['rngmin'] = -(122-rngsafemargin)
rgtlj[6]['rngmax'] = +(182-rngsafemargin)
# the 7th joint and link
rgtlj[7]['name'] = 'link7'
rgtlj[7]['mother'] = 6
rgtlj[7]['child'] = 8
rgtlj[7]['linkpos'] = rgtlj[6]['linkend']
rgtlj[7]['linkvec'] = np.array([0,0,0])
rgtlj[7]['rotax'] = np.array([1,0,0])
rgtlj[7]['rotangle'] = 0
rgtlj[7]['rotmat'] = np.dot(rgtlj[6]['rotmat'], rm.rodrigues(rgtlj[7]['rotax'], rgtlj[7]['rotangle']))
rgtlj[7]['linkend'] = np.dot(rgtlj[7]['rotmat'], rgtlj[7]['linkvec'])+rgtlj[7]['linkpos']
rgtlj[7]['rngmin'] = -(47-rngsafemargin)
rgtlj[7]['rngmax'] = +(70-rngsafemargin)
# rgtlj[7]['rngmin'] = -(87-rngsafemargin)
# rgtlj[7]['rngmax'] = +(120-rngsafemargin)
# the 8th joint and link
rgtlj[8]['name'] = 'link8'
rgtlj[8]['mother'] = 7
rgtlj[8]['child'] = 9
rgtlj[8]['linkpos'] = rgtlj[7]['linkend']
rgtlj[8]['linkvec'] = np.array([0,7.004,-67.249])
rgtlj[8]['rotax'] = np.array([0,1,0])
rgtlj[8]['rotangle'] = 0
rgtlj[8]['rotmat'] = np.dot(rgtlj[7]['rotmat'], rm.rodrigues(rgtlj[8]['rotax'], rgtlj[8]['rotangle']))
rgtlj[8]['linkend'] = np.dot(rgtlj[8]['rotmat'], rgtlj[8]['linkvec'])+rgtlj[8]['linkpos']
rgtlj[8]['rngmin'] = -(42-rngsafemargin)
rgtlj[8]['rngmax'] = +(42-rngsafemargin)
# rgtlj[8]['rngmin'] = -(92-rngsafemargin)
# rgtlj[8]['rngmax'] = +(92-rngsafemargin)
# the 9th joint and link
rgtlj[9]['name'] = 'link9'
rgtlj[9]['mother'] = 8
rgtlj[9]['child'] = -1
rgtlj[9]['linkpos'] = rgtlj[8]['linkend']
rgtlj[9]['linkvec'] = np.array([-172.7,0,0])
rgtlj[9]['rotax'] = np.array([1,0,0])
rgtlj[9]['rotangle'] = 0
# make sure x direction faces at ee, z directions faces downward
# see the definition of the coordinates in rtq85 (execute the file)
rgtlj[9]['inherentR'] = np.dot(rm.rodrigues([1,0,0],45),rm.rodrigues([0,1,0],-90))
rgtlj[9]['rotmat'] = np.dot(np.dot(rgtlj[8]['rotmat'], rgtlj[9]['inherentR']), \
rm.rodrigues(rgtlj[9]['rotax'], rgtlj[9]['rotangle']))
rgtlj[9]['linkend'] = np.dot(rgtlj[9]['rotmat'], rgtlj[9]['linkvec'])+rgtlj[9]['linkpos']
rgtlj[9]['rngmin'] = -(300-rngsafemargin)
rgtlj[9]['rngmax'] = +(300-rngsafemargin)
return rgtlj
def removeHndcc(self, base, discretesize=8):
"""
Handcc means hand collision detection
:param discretesize: the number of hand orientations
:return:
author: weiwei
date: 20161212, tsukuba
"""
# isplotted = 0
# if self.rtq85plotlist:
# for rtq85plotnode in self.rtq85plotlist:
# rtq85plotnode.removeNode()
# self.rtq85plotlist = []
self.gripcontacts = []
self.griprotmats = []
self.gripjawwidth = []
self.gripcontactnormals = []
plotoffsetfp = 6
self.counter = 0
while self.counter < self.facetpairs.shape[0]:
# print str(self.counter) + "/" + str(self.facetpairs.shape[0]-1)
# print self.gripcontactpairs_precc
facetpair = self.facetpairs[self.counter]
facetidx0 = facetpair[0]
facetidx1 = facetpair[1]
for j, contactpair in enumerate(
self.gripcontactpairs_precc[self.counter]):
for angleid in range(discretesize):
cctpnt0 = contactpair[
0] + plotoffsetfp * self.facetnormals[facetidx0]
cctpnt1 = contactpair[
1] + plotoffsetfp * self.facetnormals[facetidx1]
cctnormal0 = self.gripcontactpairnormals_precc[
self.counter][j][0]
cctnormal1 = [
-cctnormal0[0], -cctnormal0[1], -cctnormal0[2]
]
tmphand = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# save initial hand pose
initmat = tmphand.getMat()
fgrdist = np.linalg.norm((cctpnt0 - cctpnt1))
tmphand.setJawwidth(fgrdist)
tmphand.lookAt(cctnormal0[0], cctnormal0[1], cctnormal0[2])
rotax = [0, 1, 0]
rotangle = 360.0 / discretesize * angleid
rotmat = rm.rodrigues(rotax, rotangle)
tmphand.setMat(
pandageom.cvtMat4(rotmat) * tmphand.getMat())
axx = tmphand.getMat().getRow3(0)
# 130 is the distance from hndbase to fingertip
cctcenter = (cctpnt0 + cctpnt1) / 2 + 145 * np.array(
[axx[0], axx[1], axx[2]])
tmphand.setPos(
Point3(cctcenter[0], cctcenter[1], cctcenter[2]))
# collision detection
hndbullnode = cd.genCollisionMeshMultiNp(
tmphand.handnp, base.render)
result = self.bulletworld.contactTest(hndbullnode)
if not result.getNumContacts():
self.gripcontacts.append(contactpair)
self.griprotmats.append(tmphand.getMat())
self.gripjawwidth.append(fgrdist)
self.gripcontactnormals.append(
self.gripcontactpairnormals_precc[self.counter][j])
# pg.plotDumbbell(base.render, (cctpnt0+cctpnt1)/2, cctcenter, length=245, thickness=5, rgba=[.4,.4,.4,1])
# pg.plotAxisSelf(base.render, (cctpnt0+cctpnt1)/2+245*np.array([axx[0], axx[1], axx[2]]),
# tmprtq85.getMat(), length=30, thickness=2)
# tmprtq85.setColor([.5, .5, .5, 1])
# tmprtq85.reparentTo(base.render)
# self.rtq85plotlist.append(tmprtq85)
# isplotted = 1
# reset initial hand pose
tmphand.setMat(initmat)
self.counter += 1
self.counter = 0
def match(self, perceivedpnts, perceivednormals, ddist=5.0, dangle=30.0):
"""
do a match
:return: rotmats of top matches
author: weiwei
date: 20170802
"""
# save txt
pverts = np.array([tuple(x) for x in perceivedpnts],
dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text=True).write('perceivedpnts.ply')
# save txt
vandnarray = []
for i in range(len(self.temppnts)):
v = self.temppnts[i]
n = self.tempnormals[i]
vandn = (v[0], v[1], v[2], n[0], n[1], n[2])
vandnarray.append(vandn)
pverts = np.array(vandnarray, dtype=[('x', 'f4'),('y', 'f4'),('z', 'f4'),\
('nx','f4'),('ny','f4'),('nz','f4')])
el = PlyElement.describe(pverts, 'vertex')
PlyData([el], text=True).write('ttube.ply')
accspace = {}
# get the preceived global model descriptor
nperceivedpnts = perceivedpnts.shape[0]
i = np.argmax(perceivedpnts, axis=0)[2]
for j in range(0, nperceivedpnts):
print j, nperceivedpnts
m_0 = np.asarray(perceivedpnts[i])
m_1 = np.asarray(perceivedpnts[j])
v_m0m1 = m_0 - m_1
v_m1m0 = m_1 - m_0
n_m0 = perceivednormals[i]
n_m1 = perceivednormals[j]
# f1, namely ||d||2
f1 = np.linalg.norm(m_0 - m_1)
# f2, namely angle between n_m0 and v_m1m0
f2 = rm.degree_between(n_m0, v_m1m0)
# f3, namely angle between n_m1 and v_m0m1
f3 = rm.degree_between(n_m1, v_m0m1)
# f4, namely angle between n_m0 and n_m1
f4 = rm.degree_between(n_m0, n_m1)
# discretize the values
try:
f1d = int(math.floor(f1 / ddist) * ddist + ddist)
f2d = int(math.floor(f2 / dangle) * dangle + dangle)
f3d = int(math.floor(f3 / dangle) * dangle + dangle)
f4d = int(math.floor(f4 / dangle) * dangle + dangle)
except:
continue
key = (f1d, f2d, f3d, f4d)
# angle between n_m0 and x+
xplus = np.asarray([1, 0, 0])
yplus = np.asarray([0, 1, 0])
nm0xangle = math.degrees(rm.radian_between(n_m0, xplus))
rotax = np.cross(xplus, n_m0)
if np.isnan(rotax).any() or not rotax.any():
continue
rotmat = rm.rodrigues(rotax, nm0xangle)
v_m1m0onxplus = np.dot(v_m1m0, rotmat)
v_m1m0onxplusyzproj = np.asarray(
[0, v_m1m0onxplus[1], v_m1m0onxplus[2]])
alpha_m0 = rm.radian_between(v_m1m0onxplusyzproj, yplus)
if v_m1m0onxplus[2] < 0:
alpha_m0 = 2 * math.pi - alpha_m0
if key in self.gmd.keys():
malist = self.gmd[key]
print len(malist)
for maslot in malist:
alpha = math.degrees(alpha_m0 - maslot[2])
try:
alphadiscrete = int(
math.floor(alpha / dangle) * dangle + dangle)
except:
continue
acckey = (maslot[0], alphadiscrete)
if acckey in accspace.keys():
accspace[acckey] += 1
else:
accspace[acckey] = 1
if len(accspace.keys()) is 0:
return (None, None)
# find top matches and rot matrices
maxn = sorted(accspace.iteritems(),
key=operator.itemgetter(1),
reverse=True)[:5]
rotmat4list = []
silist = []
milist = []
for maxnele in maxn:
mi, alpha = maxnele[0]
# step1 move to temppnts[mi]
displacement0 = -self.temppnts[mi]
rotmat4_0 = Mat4.translateMat(displacement0[0], displacement0[1],
displacement0[2])
# step2 rotate to goal
normalangle = math.degrees(
rm.radian_between(self.tempnormals[mi], perceivednormals[i]))
normalrotax = np.cross(self.tempnormals[mi], perceivednormals[i])
normalrotmat = rm.rodrigues(normalrotax, normalangle)
anglerotmat = rm.rodrigues(perceivednormals[i], -alpha)
rotmat = np.dot(anglerotmat, normalrotmat)
rotmat4_1 = pg.npToMat4(rotmat)
# step3 move to perceivedpnts[i]
displacement1 = perceivedpnts[i]
rotmat4_2 = Mat4.translateMat(displacement1[0], displacement1[1],
displacement1[2])
rotmat4 = rotmat4_0 * rotmat4_1 * rotmat4_2
rotmat4list.append(rotmat4)
silist.append(i)
milist.append(mi)
return rotmat4list, silist, milist
import numpy as np
import pandaplotutils.pandactrl as pc
import pandaplotutils.pandageom as pg
import utils.robotmath as rm
base = pc.World(camp=[1000, -300, 700], lookatp=[0, 0, 0], w=1000, h=1000)
pggen = pg.PandaGeomGen()
pggen.plotAxis(base.render)
pggen.plotBox(base.render,
pos=[0, 0, 0],
x=100,
y=100,
z=50,
rgba=[1, 1, .3, 1])
#
rotmat = rm.rodrigues([1, 1, 1], 30)
pandarotmat = pg.cvtMat4(rotmat, np.array([0, 0, 0]))
pggen.plotAxis(base.render, pandamat4=pandarotmat)
base.run()
def __init__(self, jawwidth=85):
'''
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
## output
rtq85np:
the nodepath of this rtq85 hand
author: weiwei
date: 20160627
'''
self.rtq85np = NodePath("rtq85hnd")
self.handnp = self.rtq85np
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)
rtq85base.setPos(0,0,0)
# 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(pandageom.cvtMat4(rm.rodrigues([0,0,1], 90))*rtq85base.getMat())
rtq85base.reparentTo(self.rtq85np)
self.setJawwidth(jawwidth)
self.__jawwidthopen = 85.0
self.__jawwidthclosed = 0.0
def __initrgtlj(self):
"""
Init rgt arm links and joints
## note
x is facing forward, y is facing left, z is facing upward
each element of rgtlj is a dictionary
rgtlj[i]['linkpos'] indicates the position of a link
rgtlj[i]['linkvec'] indicates the vector of a link that points from start to end
rgtlj[i]['rotmat'] indicates the frame of this link
rgtlj[i]['rotax'] indicates the rotation axis of the link
rgtlj[i]['rotangle'] indicates the rotation angle of the link around the rotax
rgtlj[i]['linkend'] indicates the end position of the link (passively computed)
## more note:
rgtlj[1]['linkpos'] is the position of the first joint
rgtlj[i]['linkend'] is the same as rgtlj[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:
rgtlj:
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 10 joints
rgtlj = [dict() for i in range(10)]
rngsafemargin = 5
# the 0th link and joint
rgtlj[0]['name'] = 'link0'
rgtlj[0]['mother'] = -1
rgtlj[0]['child'] = 1
rgtlj[0]['linkpos'] = np.array([0, 0, 0])
rgtlj[0]['linkvec'] = np.array([32.0, -80.0, 542.0])
rgtlj[0]['rotax'] = np.array([0, 0, 1])
rgtlj[0]['rotangle'] = 0
rgtlj[0]['rotmat'] = np.eye(3)
rgtlj[0]['linkend'] = np.dot(rgtlj[0]['rotmat'],
rgtlj[0]['linkvec']) + rgtlj[0]['linkpos']
# the 1st joint and link
rgtlj[1]['name'] = 'link1'
rgtlj[1]['mother'] = 0
rgtlj[1]['child'] = 2
rgtlj[1]['linkpos'] = rgtlj[0]['linkend']
rgtlj[1]['linkvec'] = np.array([0, -130, 0])
rgtlj[1]['rotax'] = np.array([0, 0, 1])
rgtlj[1]['rotangle'] = 0
rgtlj[1]['rotmat'] = np.dot(
rgtlj[0]['rotmat'],
rm.rodrigues(rgtlj[1]['rotax'], rgtlj[1]['rotangle']))
rgtlj[1]['linkend'] = np.dot(rgtlj[1]['rotmat'],
rgtlj[1]['linkvec']) + rgtlj[1]['linkpos']
rgtlj[1]['rngmin'] = -(20 - rngsafemargin)
rgtlj[1]['rngmax'] = +(60 - rngsafemargin)
# the 2nd joint and link
rgtlj[2]['name'] = 'link2'
rgtlj[2]['mother'] = 1
rgtlj[2]['child'] = 3
rgtlj[2]['linkpos'] = rgtlj[1]['linkend']
rgtlj[2]['linkvec'] = np.array([0, 0, 0])
rgtlj[2]['rotax'] = np.array([0, 1, 0])
rgtlj[2]['rotangle'] = 0
rgtlj[2]['rotmat'] = np.dot(
rgtlj[1]['rotmat'],
rm.rodrigues(rgtlj[2]['rotax'], rgtlj[2]['rotangle']))
rgtlj[2]['linkend'] = np.dot(rgtlj[2]['rotmat'],
rgtlj[2]['linkvec']) + rgtlj[2]['linkpos']
rgtlj[2]['rngmin'] = -(200 - rngsafemargin)
rgtlj[2]['rngmax'] = +(70 - rngsafemargin)
# the 3rd joint and link
rgtlj[3]['name'] = 'link3'
rgtlj[3]['mother'] = 2
rgtlj[3]['child'] = 4
rgtlj[3]['linkpos'] = rgtlj[2]['linkend']
rgtlj[3]['linkvec'] = np.array([0, -20, 0])
rgtlj[3]['rotax'] = np.array([1, 0, 0])
rgtlj[3]['rotangle'] = 0
rgtlj[3]['rotmat'] = np.dot(
rgtlj[2]['rotmat'],
rm.rodrigues(rgtlj[3]['rotax'], rgtlj[3]['rotangle']))
rgtlj[3]['linkend'] = np.dot(rgtlj[3]['rotmat'],
rgtlj[3]['linkvec']) + rgtlj[3]['linkpos']
rgtlj[3]['rngmin'] = -(95 - rngsafemargin)
rgtlj[3]['rngmax'] = +(25 - rngsafemargin)
# the 4th joint and link
rgtlj[4]['name'] = 'link4'
rgtlj[4]['mother'] = 3
rgtlj[4]['child'] = 5
rgtlj[4]['linkpos'] = rgtlj[3]['linkend']
rgtlj[4]['linkvec'] = np.array([50, 0, -295.804])
rgtlj[4]['rotax'] = np.array([0, 0, 1])
rgtlj[4]['rotangle'] = 0
rgtlj[4]['rotmat'] = np.dot(
rgtlj[3]['rotmat'],
rm.rodrigues(rgtlj[4]['rotax'], rgtlj[4]['rotangle']))
rgtlj[4]['linkend'] = np.dot(rgtlj[4]['rotmat'],
rgtlj[4]['linkvec']) + rgtlj[4]['linkpos']
rgtlj[4]['rngmin'] = -(92 - rngsafemargin)
rgtlj[4]['rngmax'] = +(120 - rngsafemargin)
# the 5th joint and link
rgtlj[5]['name'] = 'link5'
rgtlj[5]['mother'] = 4
rgtlj[5]['child'] = 6
rgtlj[5]['linkpos'] = rgtlj[4]['linkend']
rgtlj[5]['linkvec'] = np.array([-50, 0, -295.804])
rgtlj[5]['rotax'] = np.array([0, 1, 0])
rgtlj[5]['rotangle'] = 0
rgtlj[5]['rotmat'] = np.dot(
rgtlj[4]['rotmat'],
rm.rodrigues(rgtlj[5]['rotax'], rgtlj[5]['rotangle']))
rgtlj[5]['linkend'] = np.dot(rgtlj[5]['rotmat'],
rgtlj[5]['linkvec']) + rgtlj[5]['linkpos']
rgtlj[5]['rngmin'] = -(180 - rngsafemargin)
rgtlj[5]['rngmax'] = -(17.2 - rngsafemargin)
# the 6th joint and link
rgtlj[6]['name'] = 'link6'
rgtlj[6]['mother'] = 5
rgtlj[6]['child'] = 7
rgtlj[6]['linkpos'] = rgtlj[5]['linkend']
rgtlj[6]['linkvec'] = np.array([0, 0, 0])
rgtlj[6]['rotax'] = np.array([0, 0, 1])
rgtlj[6]['rotangle'] = 0
rgtlj[6]['rotmat'] = np.dot(
rgtlj[5]['rotmat'],
rm.rodrigues(rgtlj[6]['rotax'], rgtlj[6]['rotangle']))
rgtlj[6]['linkend'] = np.dot(rgtlj[6]['rotmat'],
rgtlj[6]['linkvec']) + rgtlj[6]['linkpos']
rgtlj[6]['rngmin'] = -(122 - rngsafemargin)
rgtlj[6]['rngmax'] = +(182 - rngsafemargin)
# the 7th joint and link
rgtlj[7]['name'] = 'link7'
rgtlj[7]['mother'] = 6
rgtlj[7]['child'] = 8
rgtlj[7]['linkpos'] = rgtlj[6]['linkend']
rgtlj[7]['linkvec'] = np.array([0, 0, 0])
rgtlj[7]['rotax'] = np.array([1, 0, 0])
rgtlj[7]['rotangle'] = 0
rgtlj[7]['rotmat'] = np.dot(
rgtlj[6]['rotmat'],
rm.rodrigues(rgtlj[7]['rotax'], rgtlj[7]['rotangle']))
rgtlj[7]['linkend'] = np.dot(rgtlj[7]['rotmat'],
rgtlj[7]['linkvec']) + rgtlj[7]['linkpos']
rgtlj[7]['rngmin'] = -(47 - rngsafemargin)
rgtlj[7]['rngmax'] = +(70 - rngsafemargin)
# rgtlj[7]['rngmin'] = -(87-rngsafemargin)
# rgtlj[7]['rngmax'] = +(120-rngsafemargin)
# the 8th joint and link
rgtlj[8]['name'] = 'link8'
rgtlj[8]['mother'] = 7
rgtlj[8]['child'] = 9
rgtlj[8]['linkpos'] = rgtlj[7]['linkend']
rgtlj[8]['linkvec'] = np.array([0, 7.004, -67.249])
rgtlj[8]['rotax'] = np.array([0, 1, 0])
rgtlj[8]['rotangle'] = 0
rgtlj[8]['rotmat'] = np.dot(
rgtlj[7]['rotmat'],
rm.rodrigues(rgtlj[8]['rotax'], rgtlj[8]['rotangle']))
rgtlj[8]['linkend'] = np.dot(rgtlj[8]['rotmat'],
rgtlj[8]['linkvec']) + rgtlj[8]['linkpos']
rgtlj[8]['rngmin'] = -(42 - rngsafemargin)
rgtlj[8]['rngmax'] = +(42 - rngsafemargin)
# rgtlj[8]['rngmin'] = -(92-rngsafemargin)
# rgtlj[8]['rngmax'] = +(92-rngsafemargin)
# the 9th joint and link
rgtlj[9]['name'] = 'link9'
rgtlj[9]['mother'] = 8
rgtlj[9]['child'] = -1
rgtlj[9]['linkpos'] = rgtlj[8]['linkend']
rgtlj[9]['linkvec'] = np.array([-172.7, 0, 0])
rgtlj[9]['rotax'] = np.array([1, 0, 0])
rgtlj[9]['rotangle'] = 0
# make sure x direction faces at ee, z directions faces downward
# see the definition of the coordinates in rtq85 (execute the file)
rgtlj[9]['inherentR'] = np.dot(rm.rodrigues([1, 0, 0], 45),
rm.rodrigues([0, 1, 0], -90))
rgtlj[9]['rotmat'] = np.dot(np.dot(rgtlj[8]['rotmat'], rgtlj[9]['inherentR']), \
rm.rodrigues(rgtlj[9]['rotax'], rgtlj[9]['rotangle']))
rgtlj[9]['linkend'] = np.dot(rgtlj[9]['rotmat'],
rgtlj[9]['linkvec']) + rgtlj[9]['linkpos']
rgtlj[9]['rngmin'] = -(300 - rngsafemargin)
rgtlj[9]['rngmax'] = +(300 - rngsafemargin)
return rgtlj
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, 235.00, 965.00]) + 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