def get_lnk_bdmodel(robot_s, component_name, lnk_id): lnk = robot_s.manipulator_dict[component_name].lnks[lnk_id] bd_lnk = bdm.BDModel(lnk["collisionmodel"], mass=0, type="box", dynamic=False) bd_lnk.set_homomat(rm.homomat_from_posrot(lnk["gl_pos"], lnk["gl_rotmat"])) return bd_lnk
] RotMatnozeroID = [ i for i in range(len(stablecklist)) if stablecklist[i] is True ] comlist = [ comlistall[i] for i in range(len(stablecklist)) if stablecklist[i] is True ] objpath = os.path.join(this_dir, "object-1000", objname + ".stl") objpath = objpath.replace('\\', '/') objbm = [] testobj = bdm.BDModel(objpath, mass=1, restitution=restitution, dynamic=True, friction=friction, type="triangles") # testobj.set_scale((0.001,0.001,0.001)) testobj.set_rgba((.3, .5, .7, 0.5)) testobj.start_physics() testobj.set_linearDamping(0.1) testobj.set_angularDamping(0.1) objbm.append(testobj) # table plane = cm.gen_box([5.0000, 5.000, .000100], rm.homomat_from_posrot([.500, 0, -.050])) planebm = bdm.BDModel(objinit=plane, mass=0, friction=friction) planebm.set_rgba((.5, .5, .5, 1)) planebm.set_pos(np.array([0, 0, 0.600]))
this_dir, this_filename = os.path.split(__file__) base = wd.World(cam_pos=[10, 0, 5], lookat_pos=[0, 0, 1]) base.setFrameRateMeter(True) gm.gen_frame().attach_to(base) restitution = 0.5 heightlist = [0.001 * i for i in range(820, 860, 30)] errorrange_xyz = 0.003 errorrange_rpy = np.radians(2) errorconfiguration = geterrorconfiguration(errorrange_xyz, errorrange_rpy) xyz_error_print = printonscreen(pos=(-1, 0.7, 0), words="Error range on xyz: " + str(errorrange_xyz) + " (mm)") rpy_error_print = printonscreen(pos=(-1, 0.6, 0), words="Error range on rpy: " + str(errorrange_rpy) + " (degree)") # obj_box = cm.gen_box(extent=[.2, 1, .3], rgba=[.3, 0, 0, 1]) obj_box = cm.gen_sphere(radius=.5, rgba=[.3, 0, 0, 1]) obj_bd_box = bdm.BDModel(obj_box, mass=.3, type="triangles") obj_bd_box.set_pos(np.array([.7, 0, 2.7])) obj_bd_box.start_physics() base.attach_internal_update_obj(obj_bd_box) plane1 = cm.gen_box([0.30000, 0.3000, .3000100], rm.homomat_from_posrot([.500, 0, -.050])) plane1bm = bdm.BDModel(objinit=plane1, mass=1) plane1bm.set_rgba((.5, .5, .5, 1)) plane1bm.set_pos(np.array([0, 0, 1.000])) # plane1bm.start_physics() # base.attach_internal_update_obj(plane1bm) plane = cm.gen_box([5.0000, 5.000, .000100], rm.homomat_from_posrot([.500, 0, -.050])) planebm = bdm.BDModel(objinit=plane, mass=0) planebm.set_rgba((.5, .5, .5, 1)) planebm.set_pos(np.array([0, 0, 0.600]))
def getSlopeDym(self, mass=0, restitution=0, dynamic=True, friction=0.3): s = self.size if self.placement == "shu": self.slopex = trimesh.primitives.Extrusion( extrude_polygon=Polygon([ np.array([0, 0]), np.array([0.0707 * s, -70.7 * s]), np.array([0.0707 * s, 0.0707 * s]) ]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopex = bdm.BDModel(self.slopex, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopex.set_rgba((255 / 255, 51 / 255, 153 / 255, 1)) self.slopex.set_pos((0, 0, -0)) self.slopex.set_rpy(roll=0, pitch=np.radians(-45), yaw=np.radians(180)) self.slopey = trimesh.primitives.Extrusion( extrude_polygon=Polygon([ np.array([56 * s, 0]), np.array([56 * s, 100 * s]), np.array([-50 * s, 100 * s]), np.array([-50 * s, 0]) ]), extrude_transform=np.array([[1, 0, 0, -50 * s], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopey = bdm.BDModel(self.slopey, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopey.set_rgba((255 / 255, 51 / 255, 153 / 255, 1)) self.slopey.set_pos((0, 0, 0)) self.slopey.set_rpy(roll=np.radians(45), pitch=np.radians(45), yaw=np.radians(180)) self.slopez = trimesh.primitives.Extrusion( extrude_polygon=Polygon([ np.array([56 * s, 0]), np.array([56 * s, -100 * s]), np.array([-50 * s, -100 * s]), np.array([-50 * s, 0]) ]), extrude_transform=np.array([[1, 0, 0, -50 * s], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopez = bdm.BDModel(self.slopez, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopez.set_rgba((255 / 255, 51 / 255, 153 / 255, 1)) self.slopez.set_pos((0, 0, 0)) self.slopez.set_rpy(roll=np.radians(-45), pitch=np.radians(45), yaw=np.radians(0)) if self.placement == "ping": self.slopex = trimesh.primitives.Extrusion( extrude_polygon=Polygon([ np.array([0, 0]) * s, np.array([.0707, -0.0707]) * s, np.array([0.0707, 0.0707]) * s ]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.006], [0, 0, 0, 1]]), extrude_height=0.006) self.slopex = bdm.BDModel(self.slopex, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopex.set_rgba((255 / 255, 51 / 255, 153 / 255, 1)) self.slopex.set_pos((0, 0, -0)) self.slopex.set_rpy(roll=0, pitch=np.radians(-54.74), yaw=np.radians(180)) self.slopey = trimesh.primitives.Extrusion( extrude_polygon=Polygon([ np.array([0, .100]) * s, np.array([0, 0]) * s, np.array([-0.100, 0]) * s ]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.006], [0, 0, 0, 1]]), extrude_height=0.006) self.slopey = bdm.BDModel(self.slopey, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopey.set_rgba((255 / 255, 51 / 255, 153 / 255, 1)) self.slopey.set_pos((0, 0, 0)) self.slopey.set_rpy(roll=np.radians(45), pitch=np.radians(90 - 54.75), yaw=np.radians(180)) self.slopez = trimesh.primitives.Extrusion( extrude_polygon=Polygon([ np.array([0, -.100]) * s, np.array([0, 0]) * s, np.array([-.100, 0]) * s ]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.006], [0, 0, 0, 1]]), extrude_height=0.006) self.slopez = bdm.BDModel(self.slopez, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopez.set_rgba((255 / 255, 51 / 255, 153 / 255, 1)) self.slopez.set_pos((0, 0, 0)) self.slopez.set_rpy(roll=np.radians(-45), pitch=np.radians(90 - 54.75), yaw=np.radians(180)) self.slopex.start_physics() self.slopey.start_physics() self.slopez.start_physics() # self.slopex.attach_to(base) # self.slopey.attach_to(base) # self.slopez.attach_to(base) return [self.slopex, self.slopey, self.slopez]
def getSlopeDym(self,mass=0,restitution=0, dynamic=True, friction=0.3): s=self.size if self.placement=="shu": self.slopex = trimesh.primitives.Extrusion( extrude_polygon=Polygon( [np.array([0, 0]), np.array([70.7 * s, -70.7 * s]), np.array([70.7 * s, 70.7 * s])]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopex = bdm.BDModel(self.slopex, mass=mass,restitution=restitution,dynamic=dynamic, friction=friction) self.slopex.set_rgba((255 / 255, 51 / 255, 153 / 255, 1)) self.slopex.set_pos((0, 0, -0)) self.slopex.set_rpy(roll=0, pitch=-45, yaw=180) self.slopey = trimesh.primitives.Extrusion( extrude_polygon=Polygon( [np.array([56 * s, 0]), np.array([56 * s, 100 * s]), np.array([-50 * s, 100 * s]), np.array([-50 * s, 0])]), extrude_transform=np.array([[1, 0, 0, -50 * s], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopey =bdm.BDModel(self.slopey, mass=mass,restitution=restitution,dynamic=dynamic, friction=friction) self.slopey.set_rgba(r=255 / 255, g=51 / 255, b=153 / 255, a=1) self.slopey.set_pos(x=0, y=0, z=-0) self.slopey.set_rpy(roll=45, pitch=45, yaw=180) self.slopez = trimesh.primitives.Extrusion( extrude_polygon=Polygon( [np.array([56 * s, 0]), np.array([56 * s, -100 * s]), np.array([-50 * s, -100 * s]), np.array([-50 * s, 0])]), extrude_transform=np.array([[1, 0, 0, -50 * s], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopez = bdm.BDModel(self.slopez, mass=mass,restitution=restitution,dynamic=dynamic, friction=friction) self.slopez.set_rgba(r=255 / 255, g=51 / 255, b=153 / 255, a=1) self.slopez.set_pos(x=0, y=0, z=-0) self.slopez.set_rpy(roll=-45, pitch=45, yaw=0) if self.placement=="ping": self.slopex = trimesh.primitives.Extrusion( extrude_polygon=Polygon( [np.array([0, 0]) * s, np.array([70.7, -70.7]) * s, np.array([70.7, 70.7]) * s]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopex = bdm.BDModel(self.slopex, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopex.set_rgba(r=255 / 255, g=51 / 255, b=153 / 255, a=1) self.slopex.set_pos(x=0, y=0, z=-0) self.slopex.set_rpy(roll=0, pitch=-54.74, yaw=180) self.slopey = trimesh.primitives.Extrusion( extrude_polygon=Polygon( [np.array([0, 100]) * s, np.array([0, 0]) * s, np.array([-100, 0]) * s]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopey = bdm.BDModel(self.slopey, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopey.set_rgba(r=255 / 255, g=51 / 255, b=153 / 255, a=1) self.slopey.set_pos(x=0, y=0, z=-0) self.slopey.set_rpy(roll=45, pitch=90-54.75, yaw=180) self.slopez = trimesh.primitives.Extrusion( extrude_polygon=Polygon( [np.array([0, -100]) * s, np.array([0, 0]) * s, np.array([-100, 0]) * s]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -6], [0, 0, 0, 1]]), extrude_height=6) self.slopez = bdm.BDModel(self.slopez, mass=mass, restitution=restitution, dynamic=dynamic, friction=friction) self.slopez.set_rgba(r=255 / 255, g=51 / 255, b=153 / 255, a=1) self.slopez.set_pos(x=0, y=0, z=-0) self.slopez.set_rpy(roll=-45, pitch=90 - 54.75, yaw=180) self.slopex.attach_to(base.render) self.slopey.attach_to(base.render) self.slopez.attach_to(base.render) return [self.slopex,self.slopey,self.slopez]
bd_lnk_list = [] for arm_name in ["lft_arm", "rgt_arm"]: for lnk_id in [1, 2, 3, 4, 5, 6]: bd_lnk_list.append(get_lnk_bdmodel(robot_s, arm_name, lnk_id)) return bd_lnk_list if __name__ == '__main__': import os base = wd.World(cam_pos=[10, 0, 5], lookat_pos=[0, 0, 1]) base.setFrameRateMeter(True) gm.gen_frame().attach_to(base) # obj_box = cm.gen_box(extent=[.2, 1, .3], rgba=[.3, 0, 0, 1]) obj_box = cm.gen_sphere(radius=.5, rgba=[.3, 0, 0, 1]) obj_bd_box = bdm.BDModel(obj_box, mass=.3, type="convex") obj_bd_box.set_pos(np.array([.7, 0, 2.7])) obj_bd_box.start_physics() base.attach_internal_update_obj(obj_bd_box) robot_s = ur3ed.UR3EDual() robot_s.fk( "both_arm", np.radians(np.array([-90, -60, -60, 180, 0, 0, 90, -120, 60, 0, 0, 0]))) robot_s.gen_stickmodel().attach_to(base) robot_s.show_cdprimit() bd_lnk_list = get_robot_bdmoel(robot_s) for bdl in bd_lnk_list: bdl.start_physics() base.attach_internal_update_obj(bdl)
import os import numpy as np import basis import math import basis.robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.dynamics.bullet.bdmodel as bdm # base = wd.World(cam_pos=[1000, 300, 1000], lookat_pos=[0, 0, 0], toggle_debug=True) base = wd.World(cam_pos=[.3, .3, 1], lookat_pos=[0, 0, 0], toggle_debug=False) base.setFrameRateMeter(True) objpath = os.path.join(basis.__path__[0], "objects", "bunnysim.stl") # objpath = os.path.join(basis.__path__[0], "objects", "block.stl") bunnycm = bdm.BDModel(objpath, mass=1, type="box") objpath2 = os.path.join(basis.__path__[0], "objects", "bowlblock.stl") bunnycm2 = bdm.BDModel(objpath2, mass=0, type="triangles", dynamic=False) bunnycm2.set_rgba(np.array([0, 0.7, 0.7, 1.0])) bunnycm2.set_pos(np.array([0, 0, 0])) bunnycm2.start_physics() base.attach_internal_update_obj(bunnycm2) def update(bunnycm, task): if base.inputmgr.keymap['space'] is True: for i in range(1): bunnycm1 = bunnycm.copy() bunnycm1.set_mass(.1) rndcolor = np.random.rand(4) rndcolor[-1] = 1