def _toggle_jntcs(self, parentmodel, jntcs_thickness, jntcs_length=None, alpha=1): """ :param parentmodel: where to draw the frames to :return: author: weiwei date: 20201125 """ if jntcs_length is None: jntcs_length = jntcs_thickness * 15 for id in self.jlobject.tgtjnts: gm.gen_dashframe(pos=self.jlobject.jnts[id]['gl_pos0'], rotmat=self.jlobject.jnts[id]['gl_rotmat0'], length=jntcs_length, thickness=jntcs_thickness, alpha=alpha).attach_to(parentmodel) gm.gen_frame(pos=self.jlobject.jnts[id]['gl_posq'], rotmat=self.jlobject.jnts[id]['gl_rotmatq'], length=jntcs_length, thickness=jntcs_thickness, alpha=alpha).attach_to(parentmodel)
def _toggle_tcpcs(self, parentmodel, tcp_jntid, tcp_localpos, tcp_localrotmat, tcpic_rgba, tcpic_thickness, tcpcs_thickness=None, tcpcs_length=None): """ :param parentmodel: where to draw the frames to :param tcp_jntid: single id or a list of ids :param tcp_localpos: :param tcp_localrotmat: :param tcpic_rgba: color that used to render the tcp indicator :param tcpic_thickness: thickness the tcp indicator :param tcpcs_thickness: thickness the tcp coordinate frame :return: author: weiwei date: 20201125 """ if tcp_jntid is None: tcp_jntid = self.jlobject.tcp_jntid if tcp_localpos is None: tcp_localpos = self.jlobject.tcp_localpos if tcp_localrotmat is None: tcp_localrotmat = self.jlobject.tcp_localrotmat if tcpcs_thickness is None: tcpcs_thickness = tcpic_thickness if tcpcs_length is None: tcpcs_length = tcpcs_thickness * 15 tcp_globalpos, tcp_globalrotmat = self.jlobject.get_gl_tcp( tcp_jntid, tcp_localpos, tcp_localrotmat) if isinstance(tcp_globalpos, list): for i, jid in enumerate(tcp_jntid): jgpos = self.jlobject.joints[jid]['g_posq'] gm.gen_dumbbell(spos=jgpos, epos=tcp_globalpos[i], thickness=tcpic_thickness, rgba=tcpic_rgba).attach_to(parentmodel) gm.gen_frame(pos=tcp_globalpos[i], rotmat=tcp_globalrotmat[i], length=tcpcs_length, thickness=tcpcs_thickness, alpha=1).attach_to(parentmodel) else: jgpos = self.jlobject.joints[tcp_jntid]['g_posq'] gm.gen_dumbbell(spos=jgpos, epos=tcp_globalpos, thickness=tcpic_thickness, rgba=tcpic_rgba).attach_to(parentmodel) gm.gen_frame(pos=tcp_globalpos, rotmat=tcp_globalrotmat, length=tcpcs_length, thickness=tcpcs_thickness, alpha=1).attach_to(parentmodel)
import visualization.panda.world as wd import modeling.geometric_model as gm import pickle import vision.rgb_camera.util_functions as rgbu import cv2.aruco as aruco import numpy as np import basis.robot_math as rm origin, origin_rotmat = pickle.load(open("origin.pkl", "rb")) origin_homomat = np.eye(4) origin_homomat[:3, :3] = origin_rotmat origin_homomat[:3, 3] = origin print(origin, origin_rotmat) # base = wd.World(cam_pos=origin, lookat_pos=origin_rotmat[:,0]-origin_rotmat[:,1]+origin_rotmat[:,2]) base = wd.World(cam_pos=np.zeros(3), lookat_pos=np.array([0, 0, 10])) gm.gen_frame(length=1, thickness=.1).attach_to(base) # base.run() pk_obj = pk.PyKinectAzure() pk_obj.device_open() pk_obj.device_start_cameras() gm.gen_frame(pos=origin, rotmat=origin_rotmat, length=1, thickness=.03).attach_to(base) # base.run() # pcd_list = [] # marker_center_list = [] # 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:
def debugpos(pos, rot, base): gm.gen_frame(pos, rot).attach_to(base)
# kadai02_01.py # 2021/04/30 実験C第二回の課題1 # glassと星形のモデルを作成し,glass1(黄色)とstar1(黄色)間,star1(黄色)とstar2(紫色)間,glass2(紫色)とstar2(紫色)間の干渉をチェックしました. # 凸包の三角メッシュのものは黄色で,もともとの三角メッシュのものは紫色で表示しました. import math import numpy as np from basis import robot_math as rm import modeling.geometric_model as gm import modeling.collision_model as cm import visualization.panda.world as wd if __name__ == '__main__': base = wd.World(cam_pos=np.array([.6, .3, .8]), lookat_pos=np.zeros(3)) gm.gen_frame(length=.2, thickness=.01).attach_to(base) # グローバル座標系 # glassと星形のモデルのファイルを用いてCollisionModelを初期化します # glass1, star1はmesh(convex_hull)の設定で作成します glass1 = cm.CollisionModel(initor="objects/glass1.stl", cdprimit_type="surface_balls", cdmesh_type="convex_hull") glass1.set_rgba([.9, .75, .35, 1]) # 黄色に変更 glass1.set_pos(np.array([0, -.06, 0])) star1 = cm.CollisionModel(initor="objects/star2.stl", cdprimit_type="surface_balls", cdmesh_type="convex_hull") star1.set_rgba([.9, .75, .35, 1]) # 黄色に変更 star1.set_pos(np.array([0, .01, .07])) star1.set_rotmat(rm.rotmat_from_axangle(axis=[0, 0, 1], angle=math.pi / 2.))
import numpy as np import modeling.geometric_model as gm import visualization.panda.world as wd import basis.robot_math as rm import math 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)
new_arm_tcp_rotmat = rel_rotmat.dot(current_arm_tcp_rotmat) last_jnt_values = rbt_s.get_jnt_values() new_jnt_values = rbt_s.ik(tgt_pos=new_arm_tcp_pos, tgt_rotmat=new_arm_tcp_rotmat, seed_jnt_values=current_jnt_values) # if new_jnt_values is None: # continue rbt_s.fk(jnt_values=new_jnt_values) toc = time.time() start_frame_id = math.ceil((toc - tic) / .01) # rbt_x.arm_move_jspace_path([last_jnt_values, new_jnt_values], time_interval=.1, start_frame_id=start_frame_id) onscreen.append(rbt_s.gen_meshmodel()) onscreen[-1].attach_to(base) # print(pre_pos) # print(onscreen) operation_count += 1 # time.sleep(1/30) return task.cont if __name__ == '__main__': gm.gen_frame(length=3, thickness=0.01).attach_to(base) # threading.Thread(target=agv_move, args=()).start() taskMgr.doMethodLater(1 / 60, agv_move, "agv_move", extraArgs=None, appendTask=True) base.run()
import visualization.panda.world as wd import modeling.collision_model as cm import grasping.planning.antipodal as gpa import robot_sim.end_effectors.grippers.yumi_gripper.yumi_gripper as yg import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as hnde from direct.gui.OnscreenText import OnscreenText from panda3d.core import TextNode import basis.robot_math as rm import modeling.geometric_model as gm base = wd.World(cam_pos=[0.06, 0.03, 0.09], w=960, h=540, lookat_pos=[0, 0, 0.0]) gm.gen_frame( length=.01, thickness=.0005, ).attach_to(base) # p=[[ 5.00000000e+00,8.00000000e+00, -1.00000000e+01], # [ 5.00000000e+00, -8.00000000e+00, 1.00000000e+01], # [-2.60197991e-12, -1.00000000e+00, -3.99900278e-07], # [-2.60197991e-12, 1.00000000e+00, 3.99900278e-07], # [-1.00000000e-07, -1.00000000e+01, 0.00000000e+00], # [-1.00000000e-07, 1.00000000e+01, 0.00000000e+00]] p = [[0, 0, 0], [0, .100, 0], [0, 0, .100], [.100, 0, 0], [.100, .100, .100], [.100, .100, 0], [.100, 0, .100], [0, .100, .100]] m = [[.050, 0, 0], [.050, .150, 0], [.050, 0, .150], [.150, 0, 0], [.150, .150, .150], [.150, .150, 0], [.150, 0, .150], [.050, .150, .150]] # mesh = trimesh.Trimesh(p).convex_hull # body=mesh.convex_hull # b = cm.CollisionModel(mesh)
import time import math import numpy as np from basis import robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.xarm_shuidi.xarm_shuidi as xss base = wd.World(cam_pos=[3, 1, 2], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) # object object = cm.CollisionModel("./objects/bunnysim.stl") object.set_pos(np.array([.85, 0, .37])) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) # robot_s component_name = 'arm' robot_s = xss.XArmShuidi() robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base) # base.run() seed_jnt_values = robot_s.get_jnt_values(component_name=component_name) for y in range(-5, 5): tgt_pos = np.array([.3, y * .1, 1]) tgt_rotmat = rm.rotmat_from_euler(0, math.pi / 2, 0) gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base) tic = time.time() jnt_values = robot_s.ik(component_name=component_name, tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat, max_niter=500,
import numpy as np import math, time import basis.robot_math as rm import visualization.panda.world as wd import robot_sim.robots.yumi.yumi as ym import modeling.geometric_model as gm if __name__ == "__main__": base = wd.World(cam_pos=[3, 1, 1], lookat_pos=[0, 0, 0.5]) gm.gen_frame(length=.2).attach_to(base) yumi_s = ym.Yumi(enable_cc=True) # ik test manipulator_name = 'rgt_arm' tgt_pos = np.array([.1, -.5, .3]) tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2) tgt_pos1 = np.array([.4, -.35, .45]) tgt_rotmat1 = tgt_rotmat # tgt_rotmat1 = rm.rotmat_from_axangle([0, 1, 0], -math.pi / 3).dot(tgt_rotmat) tgt_pos2 = np.array([.6, -.2, .6]) tgt_rotmat2 = tgt_rotmat # tgt_rotmat2 = rm.rotmat_from_axangle([0, 1, 0], -math.pi/6).dot(tgt_rotmat1) gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base) tic = time.time() # jnt_values = robot_s.ik(hnd_name, tgt_pos, tgt_rotmat, seed_jnt_values=np.array([.0,.0,.0,.0,.0,.0,.0])) jnt_values = yumi_s.ik(manipulator_name, tgt_pos, tgt_rotmat) # jnt_values1 = robot_s.ik(hnd_name, tgt_pos1, tgt_rotmat1) # jnt_values2 = robot_s.ik(hnd_name, tgt_pos2, tgt_rotmat2) jnt_values1 = yumi_s.ik(manipulator_name, tgt_pos1, tgt_rotmat1, seed_jnt_values=jnt_values)
def showbaseframe(self, length=0.10, thickness=0.005): gm.gen_frame(length=length, thickness=thickness).attach_to(base)
import math import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85 import grasping.annotation.utils as gu import pickle base = wd.World(cam_pos=[.3, .3, .3], lookat_pos=[0, 0, 0]) gm.gen_frame(length=.05, thickness=.0021).attach_to(base) # object object_bunny = cm.CollisionModel("objects/bunnysim.stl") object_bunny.set_rgba([.9, .75, .35, .3]) object_bunny.attach_to(base) # 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)
import visualization.panda.world as wd import modeling.geometric_model as gm import basis.robot_math as rm import math import numpy as np base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) frame_o = gm.gen_frame(length=.2) frame_o.attach_to(base) # rotmat = rm.rotmat_from_axangle([1,1,1],math.pi/4) rotmat_a = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) # frame_a = gm.gen_mycframe(length=.2, rotmat=rotmat) frame_a = gm.gen_dashframe(length=.2, rotmat=rotmat_a) frame_a.attach_to(base) # point in a pos_a = np.array([.15, .07, .05]) # pos_start = rotmat_a.dot(pos_a) # pos_end = rotmat_a.dot(np.array([pos_a[0], pos_a[1], 0])) # # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # gm.gen_stick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) # pos_start = rotmat_a.dot(np.array([pos_a[0], pos_a[1], 0])) # pos_end = rotmat_a.dot(np.array([pos_a[0], 0, 0])) # 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([pos_a[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) # gm.gen_stick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) # pos_start = rotmat_a.dot(np.array([pos_a[0], 0, pos_a[2]]))
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.ur3_dual.ur3_dual as ur3d import math import numpy as np import basis.robot_math as rm if __name__ == '__main__': base = wd.World(cam_pos=[9, 4, 4], lookat_pos=[0, 0, .7]) gm.gen_frame(length=.7, thickness=.02).attach_to(base) # object object = cm.CollisionModel("./objects/bunnysim.stl") object.set_pos(np.array([.55, -.3, 1.3])) object.set_rotmat( rm.rotmat_from_euler(-math.pi / 3, math.pi / 6, math.pi / 9)) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) gm.gen_frame(length=.3, thickness=.015).attach_to(object) # robot_s robot_s = ur3d.UR3Dual() robot_meshmodel = robot_s.gen_meshmodel(rgba=[.3, .3, .3, .3]) robot_meshmodel.attach_to(base) base.run()
import opt_ik base = wd.World(cam_pos=[-15, 2.624 - 0.275, 15], lookat_pos=[-1.726 - 0.35, 2.624 - 0.275, 5.323], auto_cam_rotate=False) mcn_s = mcn.TBM() mcn_s.fk(np.array([math.pi / 60])) mcn_s.gen_meshmodel().attach_to(base) rbt_s = rbt.TBMChanger7R(pos=np.array([-1.726 - 0.35, 2.624 - 0.275, 5.323])) # rbt_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base) # ik_s = opt_ik.OptIK(rbt_s, component_name='arm', obstacle_list=[]) # base.run() for cutter in mcn_s.cutters['0.75']: tgt_pos = cutter.pos tgt_rotmat = rm.rotmat_from_euler(math.pi / 2, 0, 0).dot(cutter.rotmat) gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat, thickness=.05, length=1).attach_to(base) seed0 = np.zeros(7) seed0[3] = math.pi / 2 seed0[4] = math.pi / 2 seed0[5] = -math.pi / 2 seed1 = np.zeros(7) seed1[3] = -math.pi / 2 seed1[4] = -math.pi / 2 seed1[5] = -math.pi / 2 # try: # jnt_values, _ = ik_s.solve(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat, seed_jnt_values= seed0) # except: # jnt_values = None # if jnt_values is None: # try: # jnt_values, _ = ik_s.solve(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat, seed_jnt_values= seed1)
# c1 = cm.gen_box(extent=[.006 * 20, 0.006 * 30, .001], homomat=rm.homomat_from_posrot([0.006 * 19, 0.006 * 4.5, 0], # rm.rotmat_from_axangle( # [0, 0, 1], # 0 * np.pi / 2)), # rgba=[0, 0, 0, 0.2]) # c2 = cm.gen_box(extent=[.006 * 3, 0.006 * 3, .001], homomat=rm.homomat_from_posrot([0.006 * 4, 0.006 * 12, 0], # rm.rotmat_from_axangle( # [0, 0, 1], # 0 * np.pi / 2)), # rgba=[0, 0, 0, 0.2]) cut_list = [c1, c2, c3, c4, c5, c6, c7, c8] # cut_list = [c1] gm.gen_frame( pos=matrix[0][0], length=.01, thickness=.0005, ).attach_to(base) # cut_list = [] for model in cut_list: model.attach_to(base) grid = Grid(np.array(matrix), interval) node = Node(grid, height=0.006, origin_offset=0.001) matrix_infos = node.node_matrix_infos for key in matrix_infos.keys(): element = Element(matrix_infos[key], radius=0.0006, id=key, cut=cut_list, support=True) # element.get_stl()
self.win.requestProperties(win_props) def set_origin(self, origin: np.ndarray): """ :param origin: 1x2 np array describe the left top corner of the window """ win_props = WindowProperties() win_props.setOrigin(origin[0], origin[1]) self.win.requestProperties(win_props) if __name__ == "__main__": import modeling.geometric_model as gm base = wd.World(cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2]) gm.gen_frame(length=.2).attach_to(base) # extra window 1 ew = ExtraWindow(base, cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2]) ew.set_origin((0, 40)) # ImgOnscreen() img = cv2.imread("img.png") on_screen_img = ImgOnscreen(img.shape[:2][::-1], parent_np=ew) on_screen_img.update_img(img) # extra window 2 ew2 = ExtraWindow(base, cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2]) ew2.set_origin((0, ew.size[1])) gm.gen_frame(length=.2).objpdnp.reparentTo(ew2.render) base.run()
def showSptAxis(self, num): axisnode = NodePath("normal") axisnode.setMat(self.p[int(num * 3)]) gm.gen_frame(length=0.050).attach_to(axisnode) axisnode.reparentTo(base.render)
# intolist = [self.jlc.lnks[4], # self.jlc.lnks[5], # self.jlc.lnks[6]] # self.cc.set_cdpair(fromlist, intolist) # fromlist = [self.jlc.lnks[3]] # intolist = [self.jlc.lnks[6]] # self.cc.set_cdpair(fromlist, intolist) if __name__ == '__main__': import time import visualization.panda.world as wd import modeling.geometric_model as gm base = wd.World(cam_pos=[-5, -5, 3], lookat_pos=[3, 0, 0]) gm.gen_frame().attach_to(base) seed0 = np.zeros(6) seed0[3] = math.pi / 2 seed1 = np.zeros(6) seed1[3] = -math.pi / 2 manipulator_instance = TBMArm(enable_cc=True) manipulator_meshmodel = manipulator_instance.gen_meshmodel(toggle_jntscs=True) manipulator_meshmodel.attach_to(base) manipulator_instance.gen_stickmodel(toggle_tcpcs=True, toggle_jntscs=True).attach_to(base) # base.run() seed_jnt_values = manipulator_instance.get_jnt_values() for x in np.linspace(1, 3, 7).tolist(): for y in np.linspace(-2, 2, 14).tolist(): print(x) tgt_pos = np.array([x, y, 0]) tgt_rotmat = np.eye(3)
import os import math import numpy as np import basis.robot_math as rm import robot_sim._kinematics.jlchain as jl import robot_sim.manipulators.manipulator_interface as mi class XArm7(mi.ManipulatorInterface): def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(7), name='xarm7', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name) # seven joints, n_jnts = 7+2 (tgt ranges from 1-7), nlinks = 7+1 jnt_saferngmargin = math.pi / 18.0 self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, .267]) self.jlc.jnts[1]['motion_rng'] = [ -math.pi + jnt_saferngmargin, math.pi - jnt_saferngmargin ] self.jlc.jnts[2]['loc_pos'] = np.array([0, 0, 0]) self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(-1.5708, 0, 0) self.jlc.jnts[2]['motion_rng'] = [ -2.18 + jnt_saferngmargin, 2.18 - jnt_saferngmargin
import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as rtq_he import modeling.geometric_model as gm if __name__ == "__main__": base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0]) gm.gen_frame(length=.2).attach_to(base) grpr = rtq_he.RobotiqHE(enable_cc=True) grpr.jaw_to(.05) grpr.gen_meshmodel(rgba=[.3,.3,.3,.3]).attach_to(base) grpr.gen_stickmodel(toggle_tcpcs=True, toggle_jntscs=True).attach_to(base) grpr.fix_to(pos=np.array([-.1, .2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05)) grpr.gen_meshmodel().attach_to(base) grpr.show_cdmesh() grpr.fix_to(pos=np.array([.1, -.2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05)) grpr.gen_meshmodel().attach_to(base) grpr.show_cdprimit() base.run()
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import math import numpy as np import basis.robot_math as rm import robot_sim.robots.cobotta.cobotta as cbt import manipulation.pick_place_planner as ppp import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[1.2, .7, 1], lookat_pos=[.0, 0, .15]) gm.gen_frame().attach_to(base) # ground ground = cm.gen_box(extent=[5, 5, 1], rgba=[.7, .7, .7, .7]) ground.set_pos(np.array([0, 0, -.51])) ground.attach_to(base) # object holder object_holder = cm.CollisionModel("objects/holder.stl") object_holder.set_rgba([.5, .5, .5, 1]) object_holder_gl_pos = np.array([-.15, -.3, .0]) object_holder_gl_rotmat = np.eye(3) obgl_start_homomat = rm.homomat_from_posrot(object_holder_gl_pos, object_holder_gl_rotmat) object_holder.set_pos(object_holder_gl_pos) object_holder.set_rotmat(object_holder_gl_rotmat) gm.gen_frame().attach_to(object_holder) object_holder_copy = object_holder.copy() object_holder_copy.attach_to(base) # object holder goal object_holder_gl_goal_pos = np.array([.25, -.05, .05])
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import math import numpy as np import basis.robot_math as rm import robot_sim.robots.xarm_shuidi.xarm_shuidi as xsm import robot_sim.end_effectors.gripper.xarm_gripper.xarm_gripper as xag import manipulation.approach_depart_planner as adp import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[2, -2, 2], lookat_pos=[.0, 0, .3]) gm.gen_frame().attach_to(base) ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7]) ground.set_pos(np.array([0, 0, -.5])) ground.attach_to(base) object_box = cm.gen_box(extent=[.02, .06, .7], rgba=[.7, .5, .3, .7]) object_box_gl_pos = np.array([.5, -.3, .35]) object_box_gl_rotmat = np.eye(3) object_box.set_pos(object_box_gl_pos) object_box.set_rotmat(object_box_gl_rotmat) gm.gen_frame().attach_to(object_box) robot_s = xsm.XArmShuidi() rrtc_s = rrtc.RRTConnect(robot_s) adp_s = adp.ADPlanner(robot_s) grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle')
Matrix([x,y,z])- def sym_axangle(ax, angle): st = sympy.sin(angle) ct = sympy.cos(angle) # ax = ax.T[0, :] _rotmat = [ [ct + ax[0] ** 2 * (1 - ct), ax[0] * ax[1] * (1 - ct) - ax[2] * st, ax[2] * ax[0] * (1 - ct) + ax[1] * st], [ax[0] * ax[1] * (1 - ct) + ax[2] * st, ct + ax[1] ** 2 * (1 - ct), ax[1] * ax[2] * (1 - ct) - ax[0] * st], [ax[2] * ax[0] * (1 - ct) - ax[1] * st, ax[1] * ax[2] * (1 - ct) + ax[0] * st, ct + ax[2] ** 2 * (1 - ct)]] return sympy.Matrix(_rotmat) base = world.World(cam_pos=np.array([1.5, 1, .7])) gm.gen_frame(alpha=.3).attach_to(base) q1, q2, q3, q4, q5, q6 = sympy.symbols("q1 q2 q3 q4 q5 q6") x, y, z, r00, r01, r02, r10, r11, r12, r20, r21, r22 = sympy.symbols("x y z r00 r01 r02 r10 r11 r12 r20 r21 r22") rbt_s = cbt_s.Cobotta() print("Computing joint 1...") # rotmat rotmat0 = Matrix(rbt_s.manipulator_dict['arm'].jnts[0]['gl_rotmatq']) ax1 = rotmat0 * Matrix(rbt_s.manipulator_dict['arm'].jnts[1]['loc_motionax']) rotmat1 = sym_axangle(ax1, q1) accumulated_rotmat1 = rotmat1 # pos pos0 = Matrix(rbt_s.manipulator_dict['arm'].jnts[0]['gl_posq']) pos1 = pos0 + rotmat0 * Matrix(rbt_s.manipulator_dict['arm'].jnts[1]['loc_pos'])
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 if __name__ == '__main__': import time import robot_sim.robots.yumi.yumi as ym import visualization.panda.world as wd import modeling.geometric_model as gm base = wd.World(cam_pos=[1.5, 0, 3], lookat_pos=[0, 0, .5]) gm.gen_frame().attach_to(base) yumi_instance = ym.Yumi(enable_cc=True) component_name = 'rgt_arm' start_pos = np.array([.5, -.3, .3]) start_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2) goal_pos = np.array([.55, .3, .5]) goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2) gm.gen_frame(pos=start_pos, rotmat=start_rotmat).attach_to(base) gm.gen_frame(pos=goal_pos, rotmat=goal_rotmat).attach_to(base) inik = IncrementalNIK(yumi_instance) tic = time.time() jnt_values_list = inik.gen_linear_motion(component_name, start_tcp_pos=start_pos, start_tcp_rotmat=start_rotmat, goal_tcp_pos=goal_pos, goal_tcp_rotmat=goal_rotmat)
import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.planning.antipodal as gpa import math import numpy as np import basis.robot_math as rm import robot_sim.robots.cobotta.cobotta as cbt import manipulation.pick_place_planner as ppp import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[1.2, .7, 1], lookat_pos=[.0, 0, .15]) gm.gen_frame().attach_to(base) # ground ground = cm.gen_box(extent=[5, 5, 1], rgba=[.7, .7, .7, .7]) ground.set_pos(np.array([0, 0, -.51])) ground.attach_to(base) robot_s = cbt.Cobotta() robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base) seed_jnt_values = None for z in np.linspace(.1, .6, 5): goal_pos = np.array([.25, -.1, z]) goal_rot = rm.rotmat_from_axangle(np.array([0, 1, 0]), math.pi * 1 / 2) gm.gen_frame(goal_pos, goal_rot).attach_to(base) jnt_values = robot_s.ik(tgt_pos=goal_pos, tgt_rotmat=goal_rot, seed_jnt_values=seed_jnt_values) print(jnt_values) if jnt_values is not None: robot_s.fk(jnt_values=jnt_values) seed_jnt_values = jnt_values
import os import numpy as np import basis.robot_math as rm import robot_sim.robots.cobotta.cobotta_ripps as cbtr import robot_sim.end_effectors.gripper.cobotta_pipette.cobotta_pipette as cbtg import modeling.collision_model as cm import visualization.panda.world as wd import modeling.geometric_model as gm import utils if __name__ == '__main__': base = wd.World(cam_pos=[1.7, -1, .7], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) this_dir, this_filename = os.path.split(__file__) file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl") frame_bottom = cm.CollisionModel(file_frame) frame_bottom.set_rgba([.55, .55, .55, 1]) frame_bottom.attach_to(base) table_plate = cm.gen_box(extent=[.405, .26, .003]) table_plate.set_pos([0.07 + 0.2025, .055, .0015]) table_plate.set_rgba([.87, .87, .87, 1]) table_plate.attach_to(base) file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl") tip_rack = utils.Rack96(file_tip_rack) tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, 1]) tip_rack.set_pose(pos=np.array([.25, 0.0, .003]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)) tip_rack.attach_to(base)
import os import time import math import basis import numpy as np import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.xarm7_shuidi_mobile.xarm7_shuidi_mobile as xav if __name__ == '__main__': import copy import motion.probabilistic.rrt_connect as rrtc import visualization.panda.rpc.rviz_client as rv_client # # local code global_frame = gm.gen_frame() # define robot_s and robot_s anime info robot_s = xav.XArm7YunjiMobile() robot_meshmodel_parameters = [ None, # tcp_jntid None, # tcp_loc_pos None, # tcp_loc_rotmat False, # toggle_tcpcs False, # toggle_jntscs [0, .7, 0, .3], # rgba 'auto' ] # name # define object and object anime info objfile = os.path.join(basis.__path__[0], 'objects', 'bunnysim.stl') obj = cm.CollisionModel(objfile) obj_parameters = [[.3, .2, .1, 1]] # rgba