def scoreStabilityRefToContactNormals(constraints, show = False):
uniqconstraints = removeDuplicate(constraints)
print(uniqconstraints)
if len(uniqconstraints) == 1: # case 1
return 10
elif len(uniqconstraints) == 2:
if np.allclose(np.rad2deg(rm.angle_between_vectors(uniqconstraints[0], uniqconstraints[1])),180):
return 10 # case 2
else:
return 10 # case 3
elif len(uniqconstraints) > 2:
vertices = np.array(uniqconstraints + [np.array([0, 0, 0])])
try:
hull = ConvexHull(vertices)
numberOfFacets = len(hull.equations)
except:
hull = ConvexHull(vertices, qhull_options="QJ")
numberOfFacets = 1
if numberOfFacets == 1:
normalofvector = hull.equations[0,:3]
sidevector = functools.reduce(lambda a,b:specialMax(a,b,normalofvector), uniqconstraints)
searchspace = uniqconstraints.copy()
for indx in range(len(searchspace)):
if np.isclose(searchspace[indx], sidevector).all():
break
popid = indx
sidevector = searchspace.pop(popid)
result = [np.rad2deg(rm.angle_between_vectors(sidevector, vector)*checkSymbol(sidevector,vector,normalofvector))for vector in searchspace]
# print(result)
result= np.array(result)
if len(np.where(result<-1e-6)[0]) > 0 and len(np.where(result>1e-6)[0]):
return 2 # case 4
elif len(np.where((result<1e-6 ) & (result>-1e-6))[0]) > 0:
return 3 # case 5
else:
return 10 # case 3
else:
if minidistance_hull(np.array([0,0,0]),hull) > 1e-6:
return 0 # case 9
else:
opnormalpair = []
opnormalpairnum = 0
for vector1 in uniqconstraints[:-1]:
if any(np.array_equal(vector1, x) for x in opnormalpair):
continue
for vector2 in uniqconstraints:
if np.allclose(-vector1,vector2,atol=1e-6):
opnormalpair.append(vector1)
opnormalpair.append(vector2)
opnormalpairnum += 1
break
if 2> opnormalpairnum > 0:
return 3 # case 7
elif opnormalpairnum < 1:
return 10 # case 6
elif opnormalpairnum >1:
return 1 # case 8
def planHoldpairs(self, verticaljudge_lft=-np.cos(np.pi * 0.5 * 0.95),
verticaljudge_rgt=np.cos(np.pi * 0.5 * 0.95)):
'''
find the holding pairs considering a jig with 3 mutually perpendicular surface
:param verticaljudge_lft:
:param verticaljudge_rgt:
:return:
author: weiwei, hu zhengtao
date: 2020/04/03 osaka university
'''
# facetparis for update
updatedholdingfacetpairs = []
# find facets combinations (3)
facets_num = self.facets.shape[0]
self.temholdingpairs = list(itertools.combinations(range(facets_num), 2))
for facetpair in self.temholdingpairs:
temppairscenter = []
tempgripcontactpairnormals = []
dotnorm = np.dot(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]])
diff_angle = rm.angle_between_vectors(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]])
if diff_angle > np.radians(118) and diff_angle < np.radians(122):
# if dotnorm > verticaljudge_lft and dotnorm < verticaljudge_rgt:
updatedholdingfacetpairs.append(facetpair)
self.hpairs = updatedholdingfacetpairs
self.hpairsnum = len(self.hpairs)
def facetboundary(objtrimesh, facet, facetnormal):
"""
compute a boundary polygon for facet
assumptions:
1. there is only one boundary
2. the facet is convex
:param objtrimesh: a datatype defined in trimesh
:param facet: a data type defined in trimesh
:param facetcenter and facetnormal used to compute the transform, see trimesh.geometry.plane_transform
:return: [a list of 3d points, a shapely polygon, facetmat4 (the matrix that cvt the facet to 2d 4x4)]
author: weiwei
Chen Hao revised
"""
facetp = None
fff = False
# use -facetnormal to let the it face downward
if np.allclose(np.array(facetnormal),np.array([0,0,1]),atol=1e-4) or np.allclose(np.array(facetnormal),np.array([0,0,-1]),atol=1e-4):
facetmat4 = np.eye(4)
else:
rotatedirection = np.dot(facetnormal,[0,0,1])
if rotatedirection >1e-6:
alignaxis = np.array([0,0,1])
elif rotatedirection<-1e-6:
alignaxis = np.array([0, 0, -1])
else:
alignaxis = np.array([0, 0, 1])
# if np.array(facetnormal).sum(axis=0) >0:
# alignaxis = np.array([0, 0, 1])
# else:
# alignaxis = np.array([0, 0, -1])
fff = True
rotationaxis = np.cross(facetnormal, alignaxis)
rotationangle = rm.angle_between_vectors(facetnormal, alignaxis)
facetmat4 = np.eye(4)
# print(facetnormal)
if fff:
rotationangle = rotationangle if np.array(facetnormal).sum(axis=0) >0 else - rotationangle
facetmat4[:3,:3] = rm.rodrigues(rotationaxis, np.rad2deg(rotationangle))
facetmat4Inv = np.linalg.inv(np.matrix(facetmat4[0:3, 0:3]))
for faceidx in facet[0]:
vert0 = objtrimesh.vertices[objtrimesh.faces[faceidx][0]]
vert1 = objtrimesh.vertices[objtrimesh.faces[faceidx][1]]
vert2 = objtrimesh.vertices[objtrimesh.faces[faceidx][2]]
vert0p = rm.homotransform(facetmat4, vert0)
vert1p = rm.homotransform(facetmat4, vert1)
vert2p = rm.homotransform(facetmat4, vert2)
facep = Polygon([vert0p[:2], vert1p[:2], vert2p[:2]])
if facetp is None:
facetp = facep
# facetp = facetp.buffer(0)
else:
# facep = facep.buffer(0)
facetp = facetp.union(facep)
verts2d = list(facetp.exterior.coords)
vertsZ = vert2p[2]
return [vertsZ, verts2d, facetmat4,facetmat4Inv]
def _constraint_xangle(self, jnt_values):
self.rbt.fk(jnt_values=jnt_values, component_name=self.jlc_name)
gl_tcp_pos, gl_tcp_rotmat = self.rbt.get_gl_tcp(
manipulator_name=self.jlc_name)
delta_angle = rm.angle_between_vectors(gl_tcp_rotmat[:, 0],
self.tgt_rotmat[:, 0])
self.xangle_err.append(delta_angle)
return self._xangle_limit - delta_angle
def gen_rotmat_list(nsample=None):
rotmats = rm.gen_icorotmats(icolevel=2,
rotation_interval=math.radians(30),
crop_normal=np.array([0, 0, -1]),
crop_angle=np.pi / 3,
toggle_flat=True)
return_rotmat = []
for rotmat in rotmats:
if rm.angle_between_vectors(rotmat[:, 0], np.array([0, 0, -1])) < np.pi / 3:
return_rotmat.append(rotmat)
nreturn = len(return_rotmat)
if nsample is not None and nsample < nreturn:
return return_rotmat[0:nreturn:int(nreturn / nsample)]
return return_rotmat
spt = homomat[:3, 3]
# gm.gen_stick(spt, spt + pn_direction * 10, rgba=[0,1,0,1]).attach_to(base)
# base.run()
gm.gen_dasharrow(spt, spt - pn_direction * .07,
thickness=.004).attach_to(base) # p0
cpt, cnrml = bowl_model.ray_hit(spt,
spt + pn_direction * 10000,
option='closest')
gm.gen_dashstick(spt, cpt, rgba=[.57, .57, .57, .7],
thickness=0.003).attach_to(base)
gm.gen_sphere(pos=cpt, radius=.005).attach_to(base)
gm.gen_dasharrow(cpt, cpt - pn_direction * .07,
thickness=.004).attach_to(base) # p0
gm.gen_dasharrow(cpt, cpt + cnrml * .07, thickness=.004).attach_to(base) # p0
angle = rm.angle_between_vectors(-pn_direction, cnrml)
vec = np.cross(-pn_direction, cnrml)
rotmat = rm.rotmat_from_axangle(vec, angle)
new_plane_homomat = np.eye(4)
new_plane_homomat[:3, :3] = rotmat.dot(homomat[:3, :3])
new_plane_homomat[:3, 3] = cpt
twod_plane = gm.gen_box(np.array([.2, .2, .001]),
homomat=new_plane_homomat,
rgba=[1, 1, 1, .3])
twod_plane.attach_to(base)
new_line_segs = [
[cpt, cpt + rotmat.dot(pt_direction) * .05],
[
cpt + rotmat.dot(pt_direction) * .05,
cpt + rotmat.dot(pt_direction) * .05 + rotmat.dot(tmp_direction) * .05
],
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
homeconf=np.zeros(3),
name='cobotta',
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
# the last joint is passive
new_homeconf = self._mimic_jnt_values(homeconf)
self.jlc = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=new_homeconf,
name=name)
# six joints, n_jnts = 6+2 (tgt ranges from 1-6), nlinks = 6+1
self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, 0.024])
self.jlc.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(
0, 0, -1.570796325)
self.jlc.jnts[1]['loc_motionax'] = np.array([0, 0, 1])
self.jlc.jnts[1]['motion_rng'] = [-3.14159265, 3.14159265]
self.jlc.jnts[2]['loc_pos'] = np.array([-0.01175, 0, 0.114])
self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(
1.570796325, -0.20128231967888244, -1.570796325)
self.jlc.jnts[2]['loc_motionax'] = np.array([0, 0, 1])
self.jlc.jnts[2]['motion_rng'] = [0, 1.570796325]
self.jlc.jnts[3]['loc_pos'] = np.array([0.02699, 0.13228, -0.01175])
self.jlc.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(
0, 3.14159265, -1.24211575528)
self.jlc.jnts[3]['loc_motionax'] = np.array([0, 0, -1])
self.jlc.jnts[3]['motion_rng'] = [0, 1.570796325]
self.jlc.jnts[4]['loc_pos'] = np.array([0.07431, -0.12684, 0.0])
self.jlc.jnts[4]['loc_rotmat'] = rm.rotmat_from_euler(0, 3.14159265, 0)
self.jlc.jnts[4]['loc_motionax'] = np.array([0, 0, 1])
# self.jlc.jnts[5]['loc_pos'] = np.array([-0.0328, 0.02871, 0])
self.jlc.jnts[5]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 3.14159265)
# links
self.jlc.lnks[0]['name'] = "base"
self.jlc.lnks[0]['loc_pos'] = np.zeros(3)
self.jlc.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes",
"base_link.stl")
self.jlc.lnks[0]['rgba'] = [.5, .5, .5, 1.0]
self.jlc.lnks[1]['name'] = "link1"
self.jlc.lnks[1]['loc_pos'] = np.zeros(3)
self.jlc.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes",
"link_1.stl")
self.jlc.lnks[1]['rgba'] = [.55, .55, .55, 1.0]
self.jlc.lnks[2]['name'] = "link2"
self.jlc.lnks[2]['loc_pos'] = np.zeros(3)
self.jlc.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes",
"link_2.stl")
self.jlc.lnks[2]['rgba'] = [.15, .15, .15, 1]
self.jlc.lnks[3]['name'] = "link3"
self.jlc.lnks[3]['loc_pos'] = np.array([.0, .0, .0])
self.jlc.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes",
"link_5.stl")
self.jlc.lnks[3]['rgba'] = [.55, .55, .55, 1]
self.jlc.lnks[4]['name'] = "link4"
self.jlc.lnks[4]['loc_pos'] = np.array([.0, .0, .0])
self.jlc.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes",
"link_6.stl")
self.jlc.lnks[4]['rgba'] = [.35, .35, .35, 1.0]
self.jlc.reinitialize()
# prepare parameters for analytical ik
upper_arm_vec = self.jlc.jnts[3]['gl_posq'] - np.array([0, 0, 0.138])
lower_arm_vec = self.jlc.jnts[4]['gl_posq'] - self.jlc.jnts[3][
'gl_posq']
self._init_j2_angle = math.asin(
np.sqrt(upper_arm_vec[0]**2 + upper_arm_vec[1]**2) / 0.135)
self._init_j3_angle = rm.angle_between_vectors(lower_arm_vec,
-upper_arm_vec)
print(self._init_j3_angle, self._init_j2_angle)
# collision detection
if enable_cc:
self.enable_cc()
