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_stick(spos=np.array([.0, .0, .0]), epos=np.array([.0, .0, .1]), thickness=.005, type="round", rgba=[1, 0, 0, 1], sections=8): """ :param spos: :param epos: :param thickness: :param rgba: :return: author: weiwei date: 20210328 """ stick_sgm = gm.gen_stick(spos=spos, epos=epos, thickness=thickness, type=type, rgba=rgba, sections=sections) stick_cm = CollisionModel(stick_sgm) return stick_cm
base = wd.World(cam_pos=np.array([.7, -.7, 1]), lookat_pos=np.array([0.3, 0, 0])) gm.gen_frame().attach_to(base) sec1_spos = np.array([0, 0, 0]) sec_len = np.array([.2, 0, 0]) sec1_epos = sec1_spos + sec_len sec2_spos = sec1_epos angle = math.pi / 5.7 sec2_rotaxis = np.array([0, 0, 1]) sec2_rotmat = rm.rotmat_from_axangle(sec2_rotaxis, angle) sec2_epos = sec2_spos + sec2_rotmat.dot(sec_len) rgba = [1, .4, 0, .3] gm.gen_stick(spos=sec1_spos, epos=sec1_epos, rgba=rgba, thickness=.015).attach_to(base) gm.gen_frame(pos=sec2_spos, alpha=.2).attach_to(base) # gm.gen_stick(spos=sec2_spos, epos=sec2_spos+sec_len, rgba=rgba, thickness=.015).attach_to(base) # gm.gen_frame(pos=sec2_spos, rotmat=sec2_rotmat, alpha=.2).attach_to(base) gm.gen_stick(spos=sec2_spos, epos=sec2_epos, rgba=rgba, thickness=.015).attach_to(base) # gm.gen_circarrow(axis=sec2_rotaxis, center=sec2_rotaxis / 13+sec2_spos, starting_vector=np.array([-0,-1,0]),radius=.025, portion=.5, thickness=.003, rgba=[1,.4,0,1]).attach_to(base) # sec2_rotaxis2 = np.array([1, 0, 0]) sec2_rotmat2 = rm.rotmat_from_axangle(sec2_rotaxis2, angle) sec2_epos = sec2_spos + sec2_rotmat2.dot(sec2_epos - sec2_spos) # # sec2_rotmat2 = rm.rotmat_from_axangle([1,0,0], math.pi/6) # # sec2_epos2 = sec2_spos + sec2_rotmat.dot(np.array([.3, 0, 0])) # # rgba = [1, .4, 0, .3] # # gm.gen_stick(spos=sec1_spos, epos=sec1_epos, rgba=rgba, thickness=.015).attach_to(base) # # gm.gen_dashframe(pos=sec2_spos).attach_to(base)
if goal_joint_values_attachment is not None: rbt_s.fk(component_name=component_name, jnt_values=goal_joint_values_attachment) rbt_s.gen_meshmodel().attach_to(base) path = planner.plan(component_name=component_name, start_conf=init_joint_values, goal_conf=goal_joint_values_attachment, ext_dist=.02) n_path = len(path) previous_pos = None for id, jnt_values in enumerate(path): rbt_s.fk(component_name=component_name, jnt_values=jnt_values) rgba = rm.get_rgba_from_cmap(int(id * 256 / n_path), cm_name='cool', step=256) rgba[-1] = .3 pos, rotmat = rbt_s.get_gl_tcp(manipulator_name=component_name) if previous_pos is not None: gm.gen_stick(previous_pos, pos, rgba=rgba).attach_to(base) previous_pos = pos # rbt_s.gen_meshmodel(rgba=rgba).attach_to(base) # interpolated_confs = \ # traj_gen.interpolate_by_max_spdacc(path, # control_frequency=.008, # max_vels=max_vels) # n_path = len(interpolated_confs) # print(n_path) # previous_pos = None # for id, jnt_values in enumerate(interpolated_confs): # rbt_s.fk(component_name=component_name, jnt_values=jnt_values) # rgba = rm.get_rgba_from_cmap(int(id * 256 / n_path), cm_name='cool', step=256) # # rgba[-1] = 1 # pos, rotmat = rbt_s.get_gl_tcp(manipulator_name=component_name) # # rgbmatrix = np.eye(3)
circle_radius = .05 line_segs = [[homomat[:3, 3], homomat[:3, 3] + pt_direction * .05], [ homomat[:3, 3] + pt_direction * .05, homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05 ], [ homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05, homomat[:3, 3] + tmp_direction * .05 ], [homomat[:3, 3] + tmp_direction * .05, homomat[:3, 3]]] # gm.gen_linesegs(line_segs).attach_to(base) for sec in line_segs: gm.gen_stick(spos=sec[0], epos=sec[1], 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)
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
import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm leaf_rgba = [45 / 255, 90 / 255, 39 / 255, 1] stem_rgba = [97 / 255, 138 / 255, 61 / 255, 1] base = wd.World(cam_pos=[1, 1, .7]) sb_leaf = gm.GeometricModel(initor="objects/soybean_leaf.stl") sb_leaf.set_rgba(rgba=leaf_rgba) stem0_spos = np.array([0, 0, 0]) stem0_epos = np.array([0, 0, .05]) stem0 = gm.gen_stick(spos=stem0_spos, epos=stem0_epos, rgba=stem_rgba) stem0.attach_to(base) sbl0 = sb_leaf.copy() sbl0.set_pos(stem0_epos) sbl0.attach_to(base) sbl1 = sb_leaf.copy() sbl1.set_pos(stem0_epos - np.array([0, 0, 0.005])) sbl1.set_rotmat(rm.rotmat_from_axangle([0, 0, 1], np.pi)) sbl1.attach_to(base) base.run()
def gen_stickmodel(self, rgba=np.array([.5, 0, 0, 1]), thickness=.01, joint_ratio=1.62, link_ratio=.62, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=True, toggle_jntscs=False, toggle_connjnt=False, name='robotstick'): """ generate the stick model for a jntlnk object snp means stick nodepath :param rgba: :param tcp_jntid: :param tcp_loc_pos: :param tcp_loc_rotmat: :param toggle_tcpcs: :param toggle_jntscs: :param toggle_connjnt: draw the connecting joint explicitly or not :param name: :return: author: weiwei date: 20200331, 20201006 """ stickmodel = mc.ModelCollection(name=name) id = 0 loopdof = self.jlobject.ndof + 1 if toggle_connjnt: loopdof = self.jlobject.ndof + 2 while id < loopdof: cjid = self.jlobject.jnts[id]['child'] jgpos = self.jlobject.jnts[id]['gl_posq'] # joint global pos cjgpos = self.jlobject.jnts[cjid][ 'gl_pos0'] # child joint global pos jgmtnax = self.jlobject.jnts[id][ "gl_motionax"] # joint global rot ax gm.gen_stick(spos=jgpos, epos=cjgpos, thickness=thickness, type="rect", rgba=rgba).attach_to(stickmodel) if id > 0: if self.jlobject.jnts[id]['type'] == "revolute": gm.gen_stick(spos=jgpos - jgmtnax * thickness, epos=jgpos + jgmtnax * thickness, type="rect", thickness=thickness * joint_ratio, rgba=np.array([.3, .3, .2, rgba[3] ])).attach_to(stickmodel) if self.jlobject.jnts[id]['type'] == "prismatic": jgpos0 = self.jlobject.jnts[id]['gl_pos0'] gm.gen_stick(spos=jgpos0, epos=jgpos, type="round", thickness=thickness * joint_ratio, rgba=np.array([.2, .3, .3, rgba[3] ])).attach_to(stickmodel) id = cjid # tool center coord if toggle_tcpcs: self._toggle_tcpcs(stickmodel, tcp_jntid, tcp_loc_pos, tcp_loc_rotmat, tcpic_rgba=rgba + np.array([0, 0, 1, 0]), tcpic_thickness=thickness * link_ratio) # toggle all coord if toggle_jntscs: self._toggle_jntcs(stickmodel, jntcs_thickness=thickness * link_ratio, alpha=rgba[3]) return stickmodel
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) # gm.gen_arrow(spos=hitpos, epos=hitpos + hitnrml * .07, thickness=.002, rgba=np.array([0, 1, 0, 1])).attach_to(base) base.run()
# pos_start = rotmat_a.dot(np.array([pos_a[0], 0, pos_a[2]])) # pos_end = rotmat_a.dot(np.array([0, 0, pos_a[2]])) # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # # pos_start = rotmat_a.dot(pos_a) # # pos_end = rotmat_a.dot(np.array([0, pos_a[1], pos_a[2]])) # # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # pos_start = rotmat_a.dot(np.array([0, pos_a[1], pos_a[2]])) # pos_end = rotmat_a.dot(np.array([0, 0, pos_a[2]])) # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base) # cvt to sigma o pos_o = rotmat_a.dot(pos_a) # gm.gen_dashstick(pos_o, np.array([pos_o[0], pos_o[1], 0]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base) gm.gen_stick(pos_o, np.array([pos_o[0], pos_o[1], 0]), thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) gm.gen_dashstick(np.array([pos_o[0], pos_o[1], 0]), np.array([pos_o[0], 0, 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_stick(pos_o, np.array([pos_o[0], 0, pos_o[2]]), thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) gm.gen_dashstick(np.array([pos_o[0], 0, pos_o[2]]), np.array([pos_o[0], 0, 0]),
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, jnt_values=goal_joint_values_attachment) rbt_s.gen_meshmodel(rgba=[1, 1, 1, .9]).attach_to(base)
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
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), # rgba=[0,1,1,.5]).attach_to(base) # rectangle epos_vec = rm.unit_vector(cross_vec*.3-cross_vec.dot(ax)*ax*.3) gm.gen_stick(spos=cross_vec.dot(ax)*ax*.3-ax*.03, epos =cross_vec.dot(ax)*ax*.3-ax*.03+epos_vec*.03, rgba=[0,0,0,1], thickness=.001).attach_to(base) gm.gen_stick(spos=cross_vec.dot(ax)*ax*.3-ax*.03+epos_vec*.03, epos=cross_vec.dot(ax)*ax*.3-ax*.03+epos_vec*.03+ax*.03-ax*.005, rgba=[0,0,0,1], thickness=.001).attach_to(base) epos_vec = rm.unit_vector(nxt_vec_uvw*.3-nxt_vec_uvw.dot(ax)*ax*.3) gm.gen_stick(spos=nxt_vec_uvw.dot(ax)*ax*.3-ax*.03, epos =nxt_vec_uvw.dot(ax)*ax*.3-ax*.03+epos_vec*.03, rgba=[0,0,0,1], thickness=.001).attach_to(base) gm.gen_stick(spos=nxt_vec_uvw.dot(ax)*ax*.3-ax*.03+epos_vec*.03, epos=nxt_vec_uvw.dot(ax)*ax*.3-ax*.03+epos_vec*.03+ax*.03-ax*.0045, rgba=[0,0,0,1], thickness=.001).attach_to(base) gm.gen_torus(ax,
# box.set_rgba([153 / 255, 183 / 255, 1, 1]) # box.set_pos(np.array([0, 0, .32])) # box.set_rotmat(rm.rotmat_from_axangle([0, 1, 0], -math.pi / 12)) # box.attach_to(base) suction_s = suction.MVFLN40() loc_pos_box = np.array([.1, 0, .02]) loc_rotmat_box = rm.rotmat_from_euler(math.pi, 0, 0) gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box) gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box) suction_s.suction_to_with_scpose(gl_pos_box, gl_rotmat_box) suction_s.gen_meshmodel().attach_to(base) # gm.gen_stick( # suction_s.pos, # suction_s.pos - suction_s.rotmat[:,2]*10).attach_to(base) gm.gen_stick(suction_s.pos, np.array([0, 0, 1])).attach_to(base) # loc_pos_box = np.array([-.12, .12, .02]) # loc_rotmat_box = rm.rotmat_from_euler(math.pi*2/3, -math.pi/6, 0) loc_pos_box = np.array([-.12, .03, .02]) loc_rotmat_box = rm.rotmat_from_euler(math.pi * 5 / 7, -math.pi / 3, math.pi / 3) # loc_pos_box = np.array([-.12, .03, -.02]) # loc_rotmat_box = rm.rotmat_from_euler(0, -math.pi / 3, math.pi/3) # loc_pos_box = np.array([0, -.12, -.02]) # loc_rotmat_box = rm.rotmat_from_euler(0, math.pi / 3, math.pi/3) gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box) gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box) print(gl_pos_box) rtqgel_s = rtqgel.Robotiq85GelsightPusher()
box.set_rotmat(rm.rotmat_from_axangle([1, 0, 0], math.pi / 2)) # box.set_rotmat(rm.rotmat_from_euler(math.pi / 2, -math.pi/12, 0)) box.set_pos(np.array([.57, -.17, 1.35])) box.set_rotmat(rm.rotmat_from_euler(math.pi / 2, -math.pi / 12, 0)) suction_s = suction.MVFLN40() loc_pos_box = np.array([.1, .15, .0]) loc_rotmat_box = rm.rotmat_from_euler(math.pi / 2, 0, 0) gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box) gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box) suction_s.suction_to_with_scpose(gl_pos_box, gl_rotmat_box) suction_s.gen_meshmodel().attach_to(base) # gm.gen_stick( # suction_s.pos, # suction_s.pos - suction_s.rotmat[:,2]*10).attach_to(base) gm.gen_stick(suction_s.pos, np.array([.5, 0, 2.4])).attach_to(base) box.set_pos(np.array([.57, -.17, 1.3])) box.set_rotmat(rm.rotmat_from_axangle([1, 0, 0], math.pi / 2)) # loc_pos_box = np.array([-.125, .03, .02]) # loc_rotmat_box = rm.rotmat_from_axangle([0,1,0], math.pi*11/12).dot(rm.rotmat_from_axangle([0,0,1], math.pi/2)) # loc_rotmat_box=rm.rotmat_from_axangle([1,0,0], -math.pi/6).dot(loc_rotmat_box) # # loc_pos_box = np.array([-.12, .03, .02]) # # loc_rotmat_box = rm.rotmat_from_euler(math.pi*5/7, -math.pi / 3, math.pi/3) # # loc_pos_box = np.array([-.12, .03, -.02]) # # loc_rotmat_box = rm.rotmat_from_euler(0, -math.pi / 3, math.pi/3) # # loc_pos_box = np.array([0, -.12, -.02]) # # loc_rotmat_box = rm.rotmat_from_euler(0, math.pi / 3, math.pi/3) # gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box) # gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box) # jnt_angles = ur3d_s.ik(component_name='lft_arm', tgt_pos=gl_pos_box, tgt_rotmat=gl_rotmat_box)
def gen_stickmodel(self, rgba=np.array([.5, 0, 0, 1]), thickness=.01, jointratio=1.62, linkratio=.62, tcp_jntid=None, tcp_localpos=None, tcp_localrotmat=None, toggletcpcs=True, togglejntscs=False, togglecntjnt=False, name='robotstick'): """ generate a stick model for self.jltree_obj :param rgba: :param tcp_jntid: :param tcp_localpos: :param tcp_localrotmat: :param toggletcpcs: :param togglejntscs: :param togglecntjnt: draw the connecting joint explicitly or not :param name: :return: author: weiwei date: 20200331, 20201006, 20201205 """ stickmodel = gm.StaticGeometricModel(name=name) id = 0 loopdof = self.jlobject.ndof + 1 if togglecntjnt: loopdof = self.jlobject.ndof + 2 while id < loopdof: cjid = self.jlobject.joints[id]['child'] jgpos = self.jlobject.joints[id]['g_posq'] # joint global pos cjgpos = self.jlobject.joints[cjid][ 'g_pos0'] # child joint global pos jgmtnax = self.jlobject.joints[id][ "g_mtnax"] # joint global rot ax gm.gen_stick(spos=jgpos, epos=cjgpos, thickness=thickness, type="rect", rgba=rgba).attach_to(stickmodel) if id > 0: if self.jlobject.joints[id]['type'] == "revolute": gm.gen_stick(spos=jgpos - jgmtnax * thickness, epos=jgpos + jgmtnax * thickness, type="rect", thickness=thickness * jointratio, rgba=np.array([.3, .3, .2, 1])).attach_to(stickmodel) if self.jlobject.joints[id]['type'] == "prismatic": jgpos0 = self.jlobject.joints[id]['g_pos0'] gm.gen_stick(spos=jgpos0, epos=jgpos, type="round", hickness=thickness * jointratio, rgba=np.array([.2, .3, .3, 1])).attach_to(stickmodel) id = cjid # tool center coord if toggletcpcs: self._toggle_tcpcs(stickmodel, tcp_jntid, tcp_localpos, tcp_localrotmat, tcpic_rgba=rgba + np.array([0, 0, 1, 0]), tcpic_thickness=thickness * linkratio) # toggle all coord if togglejntscs: self._toggle_jntcs(stickmodel, jntcs_thickness=thickness * linkratio) return stickmodel