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