def gen_linear_motion(self, component_name, start_tcp_pos, start_tcp_rotmat, goal_tcp_pos, goal_tcp_rotmat, obstacle_list=[], granularity=0.03, seed_jnt_values=None, toggle_debug=False): """ :param component_name: :param start_tcp_pos: :param start_tcp_rotmat: :param goal_tcp_pos: :param goal_tcp_rotmat: :param goal_info: :param obstacle_list: :param granularity: :return: author: weiwei date: 20210125 """ jnt_values_bk = self.robot_s.get_jnt_values(component_name) pos_list, rotmat_list = rm.interplate_pos_rotmat( start_tcp_pos, start_tcp_rotmat, goal_tcp_pos, goal_tcp_rotmat, granularity=granularity) jnt_values_list = [] if seed_jnt_values is None: seed_jnt_values = jnt_values_bk for (pos, rotmat) in zip(pos_list, rotmat_list): jnt_values = self.robot_s.ik(component_name, pos, rotmat, seed_jnt_values=seed_jnt_values) if jnt_values is None: print("IK not solvable in gen_linear_motion!") self.robot_s.fk(component_name, jnt_values_bk) return None else: self.robot_s.fk(component_name, jnt_values) cd_result, ct_points = self.robot_s.is_collided( obstacle_list, toggle_contact_points=True) if cd_result: if toggle_debug: for ct_pnt in ct_points: gm.gen_sphere(ct_pnt).attach_to(base) print("Intermediate pose collided in gen_linear_motion!") self.robot_s.fk(component_name, jnt_values_bk) return None jnt_values_list.append(jnt_values) seed_jnt_values = jnt_values self.robot_s.fk(component_name, jnt_values_bk) return jnt_values_list
def gen_colps(self, radius=30, max_smp=120, show=False): nsample = int(math.ceil(self.objcm.trimesh.area / (radius**2 / 3.0))) if nsample > max_smp: nsample = max_smp samples = self.objcm.sample_surface_even(self.objcm.trimesh, nsample) samples = pcdu.trans_pcd(samples, self.objmat4) if show: for p in samples: gm.gen_sphere(pos=p, rgba=(1, 1, 0, .2), radius=radius) return samples
def gen_mesh_model(self, radius=.005, rgba=np.array([1, 0, 0, 1])) -> mc.ModelCollection: """ Plot markers for rigid body """ markers_mc = mc.ModelCollection() for i in range(len(self)): gm.gen_sphere(self.markers[i], radius=radius, rgba=rgba).attach_to(markers_mc) return markers_mc
def showCoodinate(self, id): axis1 = self.coordinate[2*id] # print("check1") # print(axis1, "11") gm.gen_mycframe(rotmat=axis1, pos=self.origin[2*id], thickness=0.01).attach_to(base) # axis2 = self.coordinate[2*id+1] # gm.gen_frame(rotmat=axis2).attach_to(base) print(self.endpairs[2*id][0],"cccc") # gm.gen_sphere(pos=self.endpairs[2*id][0], radius=0.0051).attach_to(base) # gm.gen_sphere(pos=self.endpairs[2 * id][1], radius=0.0051).attach_to(base) # gm.gen_sphere(pos=self.temporigin[2 * id], radius=0.01).attach_to(base) gm.gen_sphere(pos=self.origin[2 * id], radius=0.01).attach_to(base)
def showfeatures(self, node, normal, placementfacetpairscenter, placementVertices, placementCoM, showCoM, showNormal, showVertices): np.random.seed(0) rgb = np.random.rand(3) # color = (random.random(),random.random(),random.random(),1) color = (rgb[0], rgb[1], rgb[2], 1) # length = random.randint(60,100) length = 0.050 # self.__showSptPnt(self.holdingpairsSptPnt[i], color=color) if showNormal == True: for i in range(len(normal)): gm.gen_arrow( spos=np.array([ placementfacetpairscenter[i][0], placementfacetpairscenter[i][1], placementfacetpairscenter[i][2] ]), epos=np.array([ placementfacetpairscenter[i][0] + length * normal[i][0], placementfacetpairscenter[i][1] + length * normal[i][1], placementfacetpairscenter[i][2] + length * normal[i][2] ]), thickness=0.005, rgba=color).attach_to(node) if showCoM == True: gm.gen_sphere(pos=(placementCoM[0], placementCoM[1], placementCoM[2]), radius=.003, rgba=color).attach_to(node) gm.gen_arrow(spos=np.array( [placementCoM[0], placementCoM[1], placementCoM[2]]), epos=np.array([ placementCoM[0], placementCoM[1], placementCoM[2] - 0.100 ]), thickness=0.001, rgba=color).attach_to(node) if showVertices == True: for vertices in placementVertices: color2 = (random.random(), random.random(), random.random(), 1) for i in range(len(vertices)): gm.gen_sphere(pos=(vertices[i][0], vertices[i][1], vertices[i][2]), radius=.005, rgba=color2).attach_to(node)
def show(self, thickness = 0.0003): for y in range(self.wid_num): for x in range(self.len_num): gm.gen_sphere(pos = self.position_matrix[y][x], radius = 0.0006, rgba=(0,0,0,1)).attach_to(base) if x == self.len_num - 1 and y < self.wid_num - 1: print("check") gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x][y + 1], thickness=thickness).attach_to(base) elif y == self.wid_num - 1 and x < self.len_num-1: gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x + 1][y], thickness=thickness).attach_to(base) elif x < self.len_num-1 and y < self.wid_num - 1: gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x+1][y], thickness=thickness).attach_to( base) gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x][y+1], thickness=thickness).attach_to( base)
def gen_endsphere(self, rgba=None, name=''): """ generate an end sphere (es) to show the trajectory of the end effector :param jlobject: a JntLnk object :param rbga: color of the arm :return: null author: weiwei date: 20181003madrid, 20200331 """ eesphere = gm.StaticGeometricModel(name=name) if rgba is not None: gm.gen_sphere(pos=self.jlobject.joints[-1]['linkend'], radius=.025, rgba=rgba).attach_to(eesphere) return gm.StaticGeometricModel(eesphere)
def is_mesh_collided(self, objcm_list=[], toggle_debug=False): for i, cdelement in enumerate(self.all_cdelements): pos = cdelement['gl_pos'] rotmat = cdelement['gl_rotmat'] self.cdmesh_collection.cm_list[i].set_pos(pos) self.cdmesh_collection.cm_list[i].set_rotmat(rotmat) iscollided, collided_points = self.cdmesh_collection.cm_list[i].is_mcdwith(objcm_list, True) if iscollided: if toggle_debug: print(self.cdmesh_collection.cm_list[i].get_homomat()) self.cdmesh_collection.cm_list[i].show_cdmesh() for objcm in objcm_list: objcm.show_cdmesh() for point in collided_points: import modeling.geometric_model as gm gm.gen_sphere(point, radius=.001).attach_to(base) print("collided") return True return False
def gen_colps_top(self, show=False): col_ps = [] ps = self.pcd x_range = (min([x[0] for x in ps]), max([x[0] for x in ps])) y_range = (min([x[1] for x in ps]), max([x[1] for x in ps])) step = 10 for i in range(int(x_range[0]), int(x_range[1]) + 1, step): for j in range(int(y_range[0]), int(y_range[1]) + 1, step): ps_temp = np.asarray([ p for p in ps if i < p[0] < i + step and j < p[1] < j + step ]) if len(ps_temp) != 0: p = ps_temp[ps_temp[:, 2] == max([x[2] for x in ps_temp])][0] col_ps.append(p) col_ps = np.asarray(col_ps) if show: for p in col_ps: gm.gen_sphere(pos=p, rgba=(1, 0, 0, 1), radius=20) return col_ps
def gen_sphere(pos=np.array([0, 0, 0]), radius=0.01, rgba=[1, 0, 0, 1]): """ :param pos: :param radius: :param rgba: :return: author: weiwei date: 20161212tsukuba, 20191228osaka """ sphere_sgm = gm.gen_sphere(pos=pos, radius=radius, rgba=rgba) sphere_cm = CollisionModel(sphere_sgm) return sphere_cm
def showmultiface(self, base, num): length = 0.02 for i, facet in enumerate(self.pFaces[num]): color = (random.random(), random.random(), random.random(), 1) for j, face in enumerate(facet): # color = (random.random(), random.random(), random.random(), 1) gm.gen_sphere(face[0][0:3], radius=.005, rgba=color).attach_to(base) gm.gen_sphere(face[1][0:3], radius=.005, rgba=color).attach_to(base) gm.gen_sphere(face[2][0:3], radius=.005, rgba=color).attach_to(base) hufunc.drawanySingleSurface(base, vertices=np.array(face), color=color) gm.gen_arrow(spos=np.array([ self.pFacetpairscenter[num][i][0], self.pFacetpairscenter[num][i][1], self.pFacetpairscenter[num][i][2] ]), epos=np.array([ self.pFacetpairscenter[num][i][0] + length * self.pNormals[num][i][0], self.pFacetpairscenter[num][i][1] + length * self.pNormals[num][i][1], self.pFacetpairscenter[num][i][2] + length * self.pNormals[num][i][2] ]), thickness=0.005, rgba=color)
def update(pk_obj, pcd_list, marker_center_list, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() if len(marker_center_list) != 0: for marker_center in marker_center_list: marker_center.detach() marker_center_list.clear() pk_obj.device_get_capture() color_image_handle = pk_obj.capture_get_color_image() depth_image_handle = pk_obj.capture_get_depth_image() if color_image_handle and depth_image_handle: color_image = pk_obj.image_convert_to_numpy(color_image_handle) point_cloud = pk_obj.transform_depth_image_to_point_cloud( depth_image_handle) parameters = aruco.DetectorParameters_create() color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) corners, ids, rejectedImgPoints = aruco.detectMarkers( color_image, dictionary=aruco.Dictionary_get(aruco.DICT_4X4_250), parameters=parameters) if len(corners) == 0: return task.cont image_xy = np.mean(np.mean(corners[0], axis=0), axis=0).astype(np.int16) pcd_pnt = pk_obj.transform_color_xy_to_pcd_xyz( input_color_image_handle=color_image_handle, input_depth_image_handle=depth_image_handle, color_xy=image_xy) # cv2.circle(color_image, tuple(image_xy), 10, (255, 0, 0), -1) # cv2.imshow("test", color_image) # cv2.waitKey(0) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) marker_center = gm.gen_sphere(pos=pcd_pnt, radius=.1) marker_center.attach_to(base) marker_center_list.append(marker_center) pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() return task.cont
def update(pk_obj, pcd_list, ball_center_list, counter, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() # if len(ball_center_list) != 0: # for ball_center in ball_center_list: # ball_center.detach() # ball_center_list.clear() while True: pk_obj.device_get_capture() color_image_handle = pk_obj.capture_get_color_image() depth_image_handle = pk_obj.capture_get_depth_image() if color_image_handle and depth_image_handle: break point_cloud = pk_obj.transform_depth_image_to_point_cloud( depth_image_handle) point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud) ball = [] for id, point_cloud_sub in enumerate(point_cloud): if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[ 1] < .3 and 0.5 < point_cloud_sub[2] < 2.5: ball.append(point_cloud_sub) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) if len(ball) > 20: center = np.mean(np.array(ball), axis=0) ball_center_list.append([center, counter[0]]) ball = gm.gen_sphere(center, radius=.1) ball.attach_to(base) ball_list.append(ball) if len(ball_center_list) > 2: x = [] y = [] z = [] t = [] for cen, f_id in ball_center_list: x.append(cen[0]) y.append(cen[1]) z.append(cen[2]) t.append(f_id) para_x = np.polyfit(t, x, 1) para_y = np.polyfit(t, y, 1) para_z = np.polyfit(t, z, 2) f_x = np.poly1d(para_x) f_y = np.poly1d(para_y) f_z = np.poly1d(para_z) orbit = [] for t in np.linspace(ball_center_list[0][1], ball_center_list[0][1] + 5, 100): orbit.append(np.array([f_x(t), f_y(t), f_z(t)])) for id in range(len(orbit)): if id > 0: tmp_stick = gm.gen_stick(spos=orbit[id - 1], epos=orbit[id], thickness=.01, type="round") tmp_stick.attach_to(base) para_list.append(tmp_stick) return task.done pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() counter[0] += 1 return task.cont
robot.fk(hand_name, middle_conf_list[-1]) robot.jaw_to(hand_name, 0.03) robot_meshmodel = robot.gen_meshmodel() robot_meshmodel.attach_to(base) objb_copy = object.copy() objb_copy.set_rgba([0, 191 / 255, 1, 1]) objb_copy.set_homomat(objpose_list[-1]) objb_copy.attach_to(base) for i in range(0, len(conf_list), 2): robot.fk(hand_name, conf_list[i]) if jawwidth_list[i] < 0.04: robot.jaw_to(hand_name, jawwidth_list[i]) robot_meshmodel = robot.gen_meshmodel() tcp = robot.get_gl_tcp(hand_name) gm.gen_sphere(tcp[0], radius=0.005).attach_to(base) # robot_meshmodel.attach_to(base) # robot_attached_list.append(robot_meshmodel) obj_pose = objpose_list[i] objb_copy = object.copy() objb_copy.set_rgba([0, 191 / 255, 1, 0.08]) objb_copy.set_homomat(obj_pose) objb_copy.attach_to(base) # object_attached_list.append(objb_copy) base.run() # print(len(conf_list), conf_list) # if conf_list is None: # exit(-1) robot_attached_list = [] object_attached_list = []
def update(objbm, comlist, count, countforrot, time, objposnode, error, estimation, estimation_com, clock, task): # print(time[0]) if time[0] == 1: clock[0] = tm.time() error[0] = geterrorconfiguration(errorrange_xyz, errorrange_rpy) # objbm[0] = bdm.BDModel(objpath, mass=1000, restitution=restitution,dynamic=True, friction=.3, shapetype="triangle") objbm[0].set_homomat( computehomomat(RotMatnozero[countforrot[0]], heightlist[count[0]], error=error[0])) objbm[0].set_linearVelocity(0) objbm[0].set_angularVelocity(0) if time[0] == 10: objbm[0].set_mass(1) # base.attach_internal_update_obj(objbm[0]) objposnode[0] = printonscreen( pos=(-1, 0.4, 0), words="Initial ObjPos (Real): x=" + str(0.600 + np.round(error[0][0], decimals=5)) + " " + "y=" + str(0 + np.round(error[0][1], decimals=5)) + " " + "z=" + str(heightlist[count[0]] + np.round(error[0][1], decimals=5))) objposnode[1] = printonscreen(pos=(-1, -0.2, 0), words="Repetition: " + str(count[1] + 1) + "/" + str(repetition)) objposnode[4] = printonscreen( pos=(-1, 0.3, 0), words="Initial ObjPos (Ideal): x=" + str(0.600) + " " + "y=" + str(0) + " " + "z=" + str(heightlist[count[0]])) objposnode[5] = printonscreen( pos=(-1, 0.2, 0), words="Rot error: x axis: " + str(np.round(error[0][3], decimals=5)) + " " + "y axis: " + str(np.round(error[0][4], decimals=2)) + " " + "z axis: " + str(np.rad2deg(np.round(error[0][5], decimals=5))) + " (degree)") objposnode[6] = printonscreen(pos=(-0.5, 0.8, 0), words=str(countforrot[0]), color="navy") # estimation_com[1] = NodePath("normal") if time[0] == 320: estimation_com[0] = NodePath("normal") gm.gen_sphere(pos=np.round(np.dot(computehomomatforcom(), comlist[countforrot[0]]), decimals=4)[:3], radius=.0030, rgba=(1, 0, 0, 0.5)).attach_to(estimation_com[0]) gm.gen_sphere(pos=np.round(np.dot( objbm[0].get_homomat(), np.concatenate((objbm[0].cm.objtrm.center_mass, np.array([1])), axis=0)), decimals=4)[:3], radius=.0030, rgba=(0, 1, 0, 0.5)).attach_to(estimation_com[0]) estimation_com[0].reparentTo(base.render) # estimation_com[0].reparentTo(base.render) if time[0] == 399: if objposnode[3] is not None: objposnode[3].detachNode() if time[0] == 400: objposnode[0].detachNode() objposnode[1].detachNode() objposnode[4].detachNode() objposnode[5].detachNode() objposnode[6].detachNode() estimation_com[0].detachNode() # estimation_com[1].detachNode() objcom = np.round(np.dot( objbm[0].get_homomat(), np.concatenate((objbm[0].cm.objtrm.center_mass, np.array([1])), axis=0)), decimals=5) com = np.round(np.dot(computehomomatforcom(), comlist[countforrot[0]]), decimals=5) print(np.linalg.norm(objcom - com)) # < 0.0001 if objcom[0] <= com[0] + 0.0001 and objcom[0] >= com[0] - 0.0001 \ and objcom[1] <= com[1] + 0.0001 and objcom[1] >= com[1] - 0.0001 \ and objcom[2] <= com[2] + 0.0001 and objcom[2] >= com[2] - 0.0001: objposnode[3] = printonscreen(pos=(-0.65, 0.1, 0), words="Stable", color="green") criteria = 1 else: objposnode[3] = printonscreen(pos=(-0.65, 0.1, 0), words="Unstable!!", color="red") criteria = 0 estimation.append([ countforrot[0], RotMatnozeroID[countforrot[0]], error[0], [.600, 0, heightlist[count[0]]], objcom, criteria ]) objbm[0].set_rgba((.3, .5, .7, 0.5)) clock[1] = tm.time() print(clock[1] - clock[0]) if count[1] == repetition - 1: count[1] = 0 count[0] += 1 else: count[1] += 1 if count[0] == len(heightlist): countforrot[0] += 1 # print("Placement No.",countforrot[0]) count[0] = 0 count[1] = 0 if countforrot[0] == len(RotMatnozero): # if countforrot[0] == len(RotMatnozero): name = objname + "_estimation_3.pickle" with open(name, "wb") as file: pickle.dump(estimation, file) time[0] = 0 # if base.inputmgr.keymap['space'] is True: # objbm[0]=bdm.BDModel(objpath, mass=1, restitution=restitution, dynamic=True, friction=.2, type="triangles") # error[0] = geterrorconfiguration(errorrange_xyz, errorrange_rpy) # objbm[0].sethomomat(computehomomat(RotMatnozero[countforrot[0]], heightlist[count[0]], error=error[0])) # objbm[0].setColor(.3, .5, .7, 0.2) # base.attach_internal_update_obj(objbm[0]) time[0] += 1 return task.cont
objcm1 = cm.CollisionModel(objpath) homomat = np.eye(4) homomat[:3, :3] = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2) homomat[:3, 3] = np.array([0.02, 0.02, 0]) objcm1.set_homomat(homomat) objcm1.set_rgba([1, 1, .3, .2]) objcm2 = objcm1.copy() # objcm2= cm.gen_stick(thickness=.07) # objcm2.set_rgba([1, 0, 1, .1]) objcm2.set_pos(objcm1.get_pos() + np.array([.03, .0, .0])) # objcm1.change_cdmesh_type('convex_hull') # objcm2.change_cdmesh_type('obb') iscollided, contact_points = is_collided(objcm1.cdmesh, objcm2.cdmesh) objcm1.show_cdmesh() objcm2.show_cdmesh() objcm1.attach_to(base) objcm2.attach_to(base) print(iscollided) for ctpt in contact_points: gm.gen_sphere(ctpt, radius=.001).attach_to(base) # pfrom = np.array([0, 0, 0]) + np.array([1.0, 1.0, 1.0]) # pto = np.array([0, 0, 0]) + np.array([-1.0, -1.0, -0.9]) # rayhit_closet(pfrom=pfrom, pto=pto, objcm=objcm2) # objcm.attach_to(base) # objcm.show_cdmesh(type='box') # objcm.show_cdmesh(type='convex_hull') # gm.gen_sphere(hitpos, radius=.003, rgba=np.array([0, 1, 1, 1])).attach_to(base) # gm.gen_stick(spos=pfrom, epos=pto, thickness=.002).attach_to(base) # gm.gen_arrow(spos=hitpos, epos=hitpos + hitnrml * .07, thickness=.002, rgba=np.array([0, 1, 0, 1])).attach_to(base) base.run()
# hnd_s # contact_pairs, contact_points = gpa.plan_contact_pairs(object_bunny, # max_samples=10000, # min_dist_between_sampled_contact_points=.014, # angle_between_contact_normals=math.radians(160), # toggle_sampled_points=True) # for p in contact_points: # gm.gen_sphere(p, radius=.002).attach_to(base) # base.run() # pickle.dump(contact_pairs, open( "save.p", "wb" )) contact_pairs = pickle.load(open("save.p", "rb")) for i, cp in enumerate(contact_pairs): contact_p0, contact_n0 = cp[0] contact_p1, contact_n1 = cp[1] rgba = rm.get_rgba_from_cmap(i) gm.gen_sphere(contact_p0, radius=.002, rgba=rgba).attach_to(base) gm.gen_arrow(contact_p0, contact_p0 + contact_n0 * .01, thickness=.0012, rgba=rgba).attach_to(base) # gm.gen_arrow(contact_p0, contact_p0-contact_n0*.1, thickness=.0012, rgba = rgba).attach_to(base) gm.gen_sphere(contact_p1, radius=.002, rgba=rgba).attach_to(base) # gm.gen_dashstick(contact_p0, contact_p1, thickness=.0012, rgba=rgba).attach_to(base) gm.gen_arrow(contact_p1, contact_p1 + contact_n1 * .01, thickness=.0012, rgba=rgba).attach_to(base) # gm.gen_dasharrow(contact_p1, contact_p1+contact_n1*.03, thickness=.0012, rgba=rgba).attach_to(base) # base.run() gripper_s = rtq85.Robotiq85() contact_offset = .002
import numpy as np rotmat = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) ax, angle = rm.axangle_between_rotmat(np.eye(3), rotmat) rotmat2 = rm.rotmat_from_axangle(ax, math.pi/6) # cross_vec = rm.unit_vector(np.cross(np.array([0.1,0,1]), rotmat[:3,2])) cross_vec = rotmat2.dot(rotmat[:3,0]) base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) # base = wd.World(cam_pos=ax*2, lookat_pos=[0, 0, 0], toggle_debug=True) gm.gen_arrow(epos=ax*.3, rgba=[0,0,0,.3]).attach_to(base) gm.gen_frame(length=.2).attach_to(base) # gm.gen_dasharrow(epos=cross_vec*.3, rgba=[1,1,0,1], lsolid=.01, lspace=.0077).attach_to(base) gm.gen_arrow(epos=cross_vec*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=cross_vec*.3, rgba=[0,0,0,1]).attach_to(base) nxt_vec_uvw = rotmat2.dot(cross_vec) gm.gen_dasharrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) # gm.gen_arrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=nxt_vec_uvw*.3, rgba=[0,0,0,1]).attach_to(base) radius, _ = rm.unit_vector(cross_vec * .3 - cross_vec.dot(ax) * ax * .3, toggle_length=True) gm.gen_arrow(spos=cross_vec.dot(ax)*ax*.3, epos = cross_vec*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_dasharrow(spos=cross_vec.dot(ax)*ax*.3, epos = nxt_vec_uvw*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_arrow(epos=ax*math.sqrt(.3**2-radius**2), rgba=[0,0,0,1]).attach_to(base) ## projections # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+np.cross(ax, cross_vec*.3)*math.sin(math.pi/6), # rgba=[1,0,1,.5]).attach_to(base) # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+(cross_vec*.3-cross_vec.dot(ax)*ax*.3)*math.cos(math.pi/6),
thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) gm.gen_dashstick(np.array([0, pos_o[1], pos_o[2]]), np.array([0, pos_o[1], 0]), thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # gm.gen_dashstick(pos_o, np.array([pos_o[0], 0, pos_o[2]]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base) gm.gen_dashstick(np.array([pos_o[0], 0, pos_o[2]]), np.array([0, 0, pos_o[2]]), thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # gm.gen_dashstick(pos_o, np.array([0, pos_o[1], pos_o[2]]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base) gm.gen_dashstick(np.array([0, pos_o[1], pos_o[2]]), np.array([0, 0, pos_o[2]]), thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # # gm.gen_sphere(pos=pos_o, radius=.005, rgba=[0, 0, 0, 1]).attach_to(base) # gm.gen_dashstick(np.zeros(3), pos_o, thickness=.003, rgba=[.3, .3, .3, 1], lsolid=.01, lspace=.01).attach_to(base) gm.gen_stick(np.zeros(3), pos_o, thickness=.003, rgba=[.3, .3, .3, 1]).attach_to(base) base.run()
def showplanedfacetvertices(self, num): color = (random.random(), random.random(), random.random(), 1) for vertice in self.hpairsvertices[num]: for i in vertice: gm.gen_sphere(pos=(i[0], i[1], i[2]), radius=0.005, rgba=color).attach_to(base)
def genSphere(pos, radius=0.02, rgba=None): if rgba is None: rgba = [1, 0, 0, 1] gm.gen_sphere(pos=pos, radius=radius, rgba=rgba).attach_to(base)
# robot_s.fk(component_name="arm", jnt_values=armjnts) # is_robot_collided = robot_s.is_collided(obstacle_list=obstacle_list) # # display only if armjnts is not None and rbt_s is not collided # if is_robot_collided is False and counter is 0: # dist_pos.append(jnt_values) # counter += 1 # continue # print(dist_pos) # gpa.write_pickle_file('box', dist_pos, './', 'dist_pos.pickle') dist_pos = gpa.load_pickle_file('box', './', 'dist_pos.pickle') for i in dist_pos: pos = i.copy() pos[2] = 0 gm.gen_sphere(pos=pos).attach_to(base) ## ppp (tool ppp) object_box = tool.copy() target_obj = object_box4.copy() hnd_name = "hnd" ppp_s = ppp.PickPlacePlanner(robot_s) start_conf = robot_s.get_jnt_values(component_name="arm") goal_homomat_list = [tool_initial_homomat, tool_final_homomat] for dict_goal_pos in dist_pos: print("move to the dist pos", dict_goal_pos) robot_s.fk(component_name="agv", jnt_values=dict_goal_pos) conf_list1, jawwidth_list1, objpose_list1 = \ ppp_s.gen_pick_and_place_motion(hnd_name=hnd_name, objcm=tool, grasp_info_list=grasp_info_list_tool,
def update(pkx, rbtx, background_image, pcd_list, ball_center_list, counter, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() while True: pkx.device_get_capture() color_image_handle = pkx.capture_get_color_image() depth_image_handle = pkx.capture_get_depth_image() if color_image_handle and depth_image_handle: break point_cloud = pkx.transform_depth_image_to_point_cloud(depth_image_handle) point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud) ball = [] for id, point_cloud_sub in enumerate(point_cloud): if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[ 1] < .3 and 0.5 < point_cloud_sub[2] < 2.5: ball.append(point_cloud_sub) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) if len(ball) > 20: center = np.mean(np.array(ball), axis=0) ball_center_list.append([center, counter[0]]) ball = gm.gen_sphere(center, radius=.1) ball.attach_to(base) ball_list.append(ball) if len(ball_center_list) > 2: x = [] y = [] z = [] t = [] for cen, f_id in ball_center_list: x.append(cen[0]) y.append(cen[1]) z.append(cen[2]) t.append(f_id) para_x = np.polyfit(t, x, 1) para_y = np.polyfit(t, y, 1) para_z = np.polyfit(t, z, 2) f_x = np.poly1d(para_x) f_y = np.poly1d(para_y) f_z = np.poly1d(para_z) orbit = [] for t in np.linspace(ball_center_list[0][1], ball_center_list[0][1] + 15, 100): point = np.array([f_x(t), f_y(t), f_z(t)]) orbit.append(point) if abs(point[0]) < .7 and abs( point[1]) < .7 and abs(point[2] - 1.1) < .3: # last_point = orbit[-1] jnt_values = rbts.ik(tgt_pos=point, tgt_rotmat=np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])) if jnt_values is None: continue rbts.fk(component_name="arm", jnt_values=jnt_values) rbts.gen_meshmodel().attach_to(base) rbtx.arm_move_jspace_path( [rbtx.get_jnt_values(), jnt_values], max_jntspeed=math.pi * 1.3) break for id in range(len(orbit)): if id > 0: tmp_stick = gm.gen_stick(spos=orbit[id - 1], epos=orbit[id], thickness=.01, type="round") tmp_stick.attach_to(base) para_list.append(tmp_stick) return task.done pkx.image_release(color_image_handle) pkx.image_release(depth_image_handle) pkx.capture_release() counter[0] += 1 return task.cont
homomat[:3, :3] = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2) homomat[:3, 3] = np.array([0.02, 0.02, 0]) objcm1.set_homomat(homomat) objcm1.set_rgba([1, 1, .3, .2]) objcm2 = objcm1.copy() objcm2.set_pos(objcm1.get_pos() + np.array([.05, .02, .0])) objcm1.change_cdmesh_type('convex_hull') objcm2.change_cdmesh_type('obb') iscollided, contact_points = is_collided(objcm1, objcm2) objcm1.show_cdmesh() objcm2.show_cdmesh() objcm1.attach_to(base) objcm2.attach_to(base) print(iscollided) for ctpt in contact_points: gm.gen_sphere(ctpt, radius=.001).attach_to(base) pfrom = np.array([0, 0, 0]) + np.array([1.0, 1.0, 1.0]) # pto = np.array([0, 0, 0]) + np.array([-1.0, -1.0, -1.0]) pto = np.array([0, 0, 0]) + np.array([0.02, 0.02, 0.02]) # pfrom = np.array([0, 0, 0]) + np.array([0.0, 0.0, 1.0]) # pto = np.array([0, 0, 0]) + np.array([0.0, 0.0, -1.0]) # hit_point, hit_normal = rayhit_closet(pfrom=pfrom, pto=pto, objcm=objcm1) hit_points, hit_normals = rayhit_all(pfrom=pfrom, pto=pto, objcm=objcm1) # objcm.attach_to(base) # objcm.show_cdmesh(type='box') # objcm.show_cdmesh(type='convex_hull') # for hitpos, hitnormal in zip([hit_point], [hit_normal]): for hitpos, hitnormal in zip(hit_points, hit_normals): gm.gen_sphere(hitpos, radius=.003, rgba=np.array([0, 1, 1, 1])).attach_to(base) gm.gen_arrow(hitpos, epos=hitpos+hitnormal*.03, thickness=.002, rgba=np.array([0, 1, 1, 1])).attach_to(base) gm.gen_stick(spos=pfrom, epos=pto, thickness=.002).attach_to(base)
if id == 3: minusy_xyz = pcd_pnt_list[i] axis_x = rm.unit_vector(plusx_xyz - origin_xyz) axis_y = rm.unit_vector(origin_xyz - minusy_xyz) axis_z = rm.unit_vector(np.cross(axis_x, axis_y)) origin = origin_xyz origin_rotmat = np.eye(3) origin_rotmat[:, 0] = axis_x origin_rotmat[:, 1] = axis_y origin_rotmat[:, 2] = axis_z pickle.dump([origin, origin_rotmat], open("origin.pkl", "wb")) gm.gen_frame(pos=origin_xyz, rotmat=origin_rotmat, length=1, thickness=.03).attach_to(base) # for image_xy in image_xy_list: # cv2.circle(color_image, tuple(image_xy), 10, (255, 0, 0), -1) # cv2.imshow("test", color_image) # cv2.waitKey(0) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) for pcd_pnt in pcd_pnt_list: marker_center = gm.gen_sphere(pos=pcd_pnt, radius=.1) marker_center.attach_to(base) base.run() pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() base.run()
print("pcd_result(bw glass1 and star1):{}".format( pcd_result1)) # 衝突の結果を出力します # star1, star2 pcd_result2 = star1.is_pcdwith(star2) print("pcd_result(bw star1 and star2):{}".format( pcd_result2)) # 衝突の結果を出力します # glass2, star2 pcd_result3 = glass2.is_pcdwith(star2) print("pcd_result(bw glass2 and star2):{}".format( pcd_result3)) # 衝突の結果を出力します # mesh間の衝突を検出します # glass1, star1 mcd_result1, cd_points1 = glass1.is_mcdwith(star1, toggle_contacts=True) for pnt in cd_points1: gm.gen_sphere(pos=pnt, rgba=[1, 0, 0, 1], radius=.002).attach_to(base) print("mcd_result(bw glass1 and star1):{}".format( mcd_result1)) # 衝突の結果を出力します # star1, star2 mcd_result2, cd_points2 = star1.is_mcdwith(star2, toggle_contacts=True) for pnt in cd_points2: gm.gen_sphere(pos=pnt, rgba=[1, 0, 0, 1], radius=.002).attach_to(base) print("mcd_result(bw star1 and star2):{}".format( mcd_result2)) # 衝突の結果を出力します # glass2, star2 mcd_result3, cd_points3 = glass2.is_mcdwith(star2, toggle_contacts=True) for pnt in cd_points3: gm.gen_sphere(pos=pnt, rgba=[1, 0, 0, 1], radius=.002).attach_to(base) print("mcd_result(bw glass2 and star2):{}".format( mcd_result3)) # 衝突の結果を出力します
rgba=[0, 0, 0, 1], thickness=.002, type='round').attach_to(base) epos = (line_segs[0][1] - line_segs[0][0]) * .7 + line_segs[0][0] gm.gen_arrow(spos=line_segs[0][0], epos=epos, thickness=0.004).attach_to(base) 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 = [
rbt_s.gen_meshmodel().attach_to(base) planner = rrtc.RRTConnect(rbt_s) ee_s = cbtg.CobottaPipette() id_x = 7 id_y = 11 tip_pos, tip_rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y) z_offset = np.array([0, 0, .02]) base.change_campos_and_lookat_pos(cam_pos=[.4, .0, .15], lookat_pos=tip_pos + z_offset) spiral_points = rm.gen_3d_equilateral_verts(pos=tip_pos + z_offset, rotmat=tip_rotmat) print(spiral_points) pre_point = None for point in spiral_points: gm.gen_sphere(point, radius=.00016).attach_to(base) if pre_point is not None: gm.gen_stick(pre_point, point, thickness=.00012).attach_to(base) pre_point = point goal_joint_values_attachment = utils.search_reachable_configuration( rbt_s=rbt_s, ee_s=ee_s, component_name=component_name, tgt_pos=spiral_points[0], cone_axis=-tip_rotmat[:3, 2], rotation_interval=np.radians(15), obstacle_list=[frame_bottom], toggle_debug=False) if goal_joint_values_attachment is not None: rbt_s.fk(component_name=component_name,
def gen_circular_motion(self, component_name, circle_center_pos, circle_ax, start_tcp_rotmat, end_tcp_rotmat, radius=.02, obstacle_list=[], granularity=0.03, seed_jnt_values=None, toggle_tcp_list=False, toggle_debug=False): """ :param component_name: :param start_tcp_pos: :param start_tcp_rotmat: :param goal_tcp_pos: :param goal_tcp_rotmat: :param goal_info: :param obstacle_list: :param granularity: :return: author: weiwei date: 20210501 """ jnt_values_bk = self.robot_s.get_jnt_values(component_name) pos_list, rotmat_list = rm.interplate_pos_rotmat_around_circle( circle_center_pos, circle_ax, radius, start_tcp_rotmat, end_tcp_rotmat, granularity=granularity) # for (pos, rotmat) in zip(pos_list, rotmat_list): # gm.gen_frame(pos, rotmat).attach_to(base) # base.run() jnt_values_list = [] if seed_jnt_values is None: seed_jnt_values = jnt_values_bk for (pos, rotmat) in zip(pos_list, rotmat_list): jnt_values = self.robot_s.ik(component_name, pos, rotmat, seed_jnt_values=seed_jnt_values) if jnt_values is None: print("IK not solvable in gen_circular_motion!") self.robot_s.fk(component_name, jnt_values_bk) return [] else: self.robot_s.fk(component_name, jnt_values) cd_result, ct_points = self.robot_s.is_collided( obstacle_list, toggle_contact_points=True) if cd_result: if toggle_debug: for ct_pnt in ct_points: gm.gen_sphere(ct_pnt).attach_to(base) print("Intermediate pose collided in gen_linear_motion!") self.robot_s.fk(component_name, jnt_values_bk) return [] jnt_values_list.append(jnt_values) seed_jnt_values = jnt_values self.robot_s.fk(component_name, jnt_values_bk) if toggle_tcp_list: return jnt_values_list, [[pos_list[i], rotmat_list[i]] for i in range(len(pos_list))] else: return jnt_values_list
def __showSptPnt(self, SptPnt, color): gm.gen_sphere(pos=Vec3(SptPnt[0], SptPnt[1], SptPnt[2]), radius=20, rgba=color).attach_to(base)