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()