def get_std_convexhull(pcd,
origin="center",
color=(1, 1, 1),
transparency=1,
toggledebug=False,
toggleransac=True):
"""
create CollisionModel by pcd, standardized rotation
:param pcd:
:param origin: "center" or "tip"
:param color:
:param transparency:
:param toggledebug:
:param toggleransac:
:return: CollisionModel, position of origin
"""
rot_angle = get_rot_frompcd(pcd,
toggledebug=toggledebug,
toggleransac=toggleransac)
center = get_pcd_center(pcd)
origin_pos = np.array(center)
pcd = pcd - np.array([center]).repeat(len(pcd), axis=0)
pcd = trans_pcd(
pcd, rm.homobuild((0, 0, 0), rm.rodrigues((0, 0, 1), -rot_angle)))
convexhull = trimesh.Trimesh(vertices=pcd)
convexhull = convexhull.convex_hull
obj = cm.CollisionModel(initor=convexhull)
obj_w, obj_h = get_pcd_w_h(pcd)
if origin == "tip":
tip = get_pcd_tip(pcd, axis=0)
origin_pos = center + \
trans_pcd(np.array([tip]), rm.homobuild((0, 0, 0), rm.rodrigues((0, 0, 1), rot_angle)))[0]
pcd = pcd - np.array([tip]).repeat(len(pcd), axis=0)
convexhull = trimesh.Trimesh(vertices=pcd)
convexhull = convexhull.convex_hull
obj = cm.CollisionModel(initor=convexhull)
if toggledebug:
obj.set_rgba(color[0], color[1], color[2], transparency)
obj.reparentTo(base.render)
obj.showlocalframe()
print("Rotation angle:", rot_angle)
print("Origin:", center)
base.pggen.plotSphere(base.render, center, radius=2, rgba=(1, 0, 0, 1))
return obj, obj_w, obj_h, origin_pos, rot_angle
def checkgrasp(self):
grasp = gpa.load_pickle_file(self.objname, None, "rtqhe.pickle")
grasp_info_list = grasp
print("number of grasp 1", len(grasp))
hndfa = rtqhe.RobotiqHE(enable_cc=True)
rtqHE = copy.deepcopy(hndfa)
rtqHE.jaw_to(jawwidth=0.035)
slope = self.slopelist_high
obj = cm.CollisionModel(initor=self.objpath)
obj.set_scale((0.001, 0.001, 0.001))
cfreegrasp = []
for i, rotmat4 in enumerate(self.pRotMat):
temcfreegrasp = []
if self.collisionlist[i] == False and self.stablelist[i] == True:
obstaclelist = slope
obj.set_homomat(da.pdmat4_to_npmat4(rotmat4))
obstaclelist.append(obj)
for grasp in grasp_info_list:
jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp
rotmatgraspnp = np.dot(
da.pdmat4_to_npmat4(rotmat4),
rm.homomat_from_posrot(hnd_pos, hnd_rotmat))
rotmat4panda = da.npmat4_to_pdmat4(rotmatgraspnp)
rtqHE.fix_to(pos=rotmatgraspnp[:3, 3],
rotmat=rotmatgraspnp[:3, :3],
jawwidth=jaw_width)
if rtqHE.is_mesh_collided(obstaclelist) == False:
temcfreegrasp.append([rotmat4panda, False])
else:
temcfreegrasp.append([rotmat4panda, True])
print("grasp in collision")
cfreegrasp.append(temcfreegrasp)
print("number of grasp 2", len(temcfreegrasp))
self.cfreegrasppose = cfreegrasp
def get_obj_from_phoxiinfo_withmodel(phxilocator,
stl_f_name,
load_f_name=None,
match_filp=False,
bg_f_name="bg_0217.pkl"):
objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT +
'/obstacles/' + stl_f_name))
grayimg, depthnparray_float32, pcd = load_phxiinfo(
phoxi_f_name=load_f_name)
workingarea_uint8 = phxilocator.remove_depth_bg(depthnparray_float32,
bg_f_name=bg_f_name,
toggledebug=False)
obj_depth = phxilocator.find_paintingobj(workingarea_uint8)
if obj_depth is None:
print("painting object not detected!")
objpcd = pcdu.trans_pcd(
pcdu.remove_pcd_zeros(vu.map_depth2pcd(obj_depth, pcd)),
phxilocator.amat)
objmat4 = phxilocator.match_pcdncm(objpcd, objcm, match_rotz=match_filp)
objcm.sethomomat(objmat4)
return item.Item(objcm=objcm, pcd=objpcd, objmat4=objmat4)
def update(textNode, obj_show_node, counter, task):
if base.inputmgr.keymap['space'] is True:
print("space")
# time.sleep(0.5)
if counter[0] >= len(freehold.placementRotMat):
counter[0] = 0
if obj_show_node[0] is not None:
# obj_show_node[0].detach()
obj_show_node[1].detachNode()
obj_show_node[2].detachNode()
obj_show_node[3].detachNode()
obj_show_node[0] = cm.CollisionModel(objpath)
obj_show_node[0].set_scale((0.001, 0.001, 0.001))
obj_show_node[0].set_rgba((0, 191 / 255, 1, 0.5))
obj_show_node[0].set_homomat(freehold.placementRotMat[counter[0]])
# obj_show_node[0].attach_to(base)
obj_show_node[1] = NodePath("com")
obj_show_node[2] = NodePath("vertice")
obj_show_node[3] = NodePath("normal")
if obj_show_node[0].is_mcdwith(fixturecm):
obj_show_node[0].set_rgba((1, 0, 0, 0.5))
else:
obj_show_node[0].attach_to(base)
# gm.gen_sphere(pos = freehold.placementCoM[counter[0]]).attach_to(obj_show_node[1])
obj_show_node[1].reparentTo(base.render)
# for vertace in freehold.placementVertices[counter[0]]:
# gm.gen_sphere(pos=vertace, radius=0.005).attach_to(obj_show_node[2])
obj_show_node[2].reparentTo(base.render)
for i, normal in enumerate(freehold.placementNormals[counter[0]]):
print(normal)
gm.gen_arrow(
spos=freehold.placementfacetpairscenter[counter[0]][i],
epos=freehold.placementfacetpairscenter[counter[0]][i] +
normal * 0.1).attach_to(obj_show_node[3])
obj_show_node[3].reparentTo(base.render)
counter[0] += 1
if textNode[0] is not None:
textNode[0].detachNode()
textNode[1].detachNode()
textNode[2].detachNode()
cam_pos = base.cam.getPos()
textNode[0] = OnscreenText(text=str(cam_pos[0])[0:5],
fg=(1, 0, 0, 1),
pos=(1.0, 0.8),
align=TextNode.ALeft)
textNode[1] = OnscreenText(text=str(cam_pos[1])[0:5],
fg=(0, 1, 0, 1),
pos=(1.3, 0.8),
align=TextNode.ALeft)
textNode[2] = OnscreenText(text=str(cam_pos[2])[0:5],
fg=(0, 0, 1, 1),
pos=(1.6, 0.8),
align=TextNode.ALeft)
return task.again
def match_pcdncm_ptpt(self, target, src_stl_f_name, dv=10, show_icp=False):
obj = cm.CollisionModel(
objinit=os.path.join(config.ROOT + '/obstacles/' + src_stl_f_name))
source = np.asarray(ts.sample_surface(obj.trimesh, count=10000))
source = source[source[:, 2] > 5]
target = np.asarray(target)
rmse, fitness, homomat = o3dh.registration_ptpt(
source, target, downsampling_voxelsize=dv, toggledebug=show_icp)
return homomat
def checkcollision(self):
# checker = cdchecker.MCMchecker(toggledebug=True)
temobj = cm.CollisionModel(initor=self.objpath)
temobj.set_scale((0.001, 0.001, 0.001))
collisionlist = []
for i in range(len(self.pRotMat)):
temobj.set_homomat(da.pdmat4_to_npmat4(self.pRotMat[i]))
checkresult = temobj.is_mcdwith(self.slopelist)
# checkresult = checker.isMeshMeshListCollided(temobj, self.slopelist)
collisionlist.append(checkresult)
return collisionlist
def reconstruct_surface(pcd, radii=[.005], toggledebug=False):
print("---------------reconstruct surface bp---------------")
pcd = np.asarray(pcd)
tmmesh = o3d_helper.reconstructsurfaces_bp(pcd,
radii=radii,
doseparation=False)
obj = cm.CollisionModel(initor=tmmesh)
if toggledebug:
obj.set_rgba(1, 1, 1, 1)
obj.reparentTo(base.render)
return obj
def update(attached_list, pcd_list, counter, task):
if counter[0] >= len(pcd_list):
counter[0] = 0
if len(attached_list) != 0:
for objcm in attached_list:
objcm.detach()
attached_list.clear()
pcd = pcd_list[counter[0]]
attached_list.append(cm.CollisionModel(pcd))
attached_list[-1].attach_to(base)
counter[0]+=1
return task.again
def loadObj(f_name,
pos=(0, 0, 0),
rot=(0, 0, 0),
color=(1, 1, 1),
transparency=0.5):
obj = cm.CollisionModel(
objinit=os.path.join(config.ROOT, "obstacles", f_name))
obj.setPos(pos[0], pos[1], pos[2])
obj.setColor(color[0], color[1], color[2], transparency)
obj.setRPY(rot[0], rot[1], rot[2])
return obj
def __init__(self,
objinit,
mass=None,
restitution=0,
allowdeactivation=False,
allowccd=True,
friction=.2,
dynamic=True,
type="convex",
name="bdm"):
"""
:param objinit: GeometricModel (CollisionModel also work)
:param mass:
:param restitution:
:param allowdeactivation:
:param allowccd:
:param friction:
:param dynamic:
:param type: "convex", "triangle", "box"
:param name:
"""
if isinstance(objinit, BDModel):
self._gm = copy.deepcopy(objinit.gm)
self._bdb = objinit.bdb.copy()
self._cm = copy.deepcopy(objinit.cm)
elif isinstance(objinit, gm.GeometricModel):
if mass is None:
mass = 0
self._gm = objinit
self._bdb = bdb.BDBody(self._gm,
type,
mass,
restitution,
allow_deactivation=allowdeactivation,
allow_ccd=allowccd,
friction=friction,
dynamic=dynamic,
name=name)
else:
if mass is None:
mass = 0
self._cm = cm.CollisionModel(objinit)
self._gm = gm.GeometricModel(objinit)
self._bdb = bdb.BDBody(self._gm,
type,
mass,
restitution,
allow_deactivation=allowdeactivation,
allow_ccd=allowccd,
friction=friction,
dynamic=dynamic,
name=name)
def loadObjitem(f_name,
pos=(0, 0, 0),
rot=(0, 0, 0),
sample_num=10000,
type="box"):
if f_name[-3:] != 'stl':
f_name += '.stl'
objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT, "obstacles",
f_name),
type=type)
objmat4 = np.zeros([4, 4])
objmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz")
objmat4[:3, 3] = pos
objcm.sethomomat(objmat4)
return item.Item(objcm=objcm, objmat4=objmat4, sample_num=sample_num)
def __init__(self,z=-0.015,placement="shu",size=1.0, show=True):
self.placement=placement
self.size=size
if self.placement=="shu":
self.slopex = trimesh.primitives.Box(box_extents = (0.150,0.150,0.006))
self.slopex = cm.CollisionModel(self.slopex)
self.slopex.set_rgba((255/255,51/255,153/255, 1))
self.slopex.set_pos((0, 0, z))
self.slopex.set_rpy(roll = 0, pitch = -45, yaw = 0)
# self.slopex.reparentTo(base.render)
self.slopey = trimesh.primitives.Box( box_extents = (0.200,0.200,0.006))
self.slopey = cm.CollisionModel(self.slopey)
self.slopey.set_rgba((51/255,255/255,255/255, 1))
self.slopey.set_pos((0, 0, z))
self.slopey.set_rpy(roll = 45, pitch = 45, yaw = 0)
# self.slopey.reparentTo(base.render)
self.slopez = trimesh.primitives.Box( box_extents = (0.200,0.200,0.006))
self.slopez = cm.CollisionModel(self.slopez)
self.slopez.set_rgba((178/255,102/255,255/255, 1))
self.slopez.set_pos(( 0, 0, z))
self.slopez.set_rpy(roll = -45, pitch = 45, yaw = 0)
# self.slopez.reparentTo(base.render)
if placement == "ping":
s = self.size
self.slopex = trimesh.primitives.Extrusion(
extrude_polygon=Polygon([np.array([0, 0]), np.array([0.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=.006)
self.slopex = cm.CollisionModel(self.slopex)
self.slopex.set_rgba((51 / 255, 255 / 255, 255 / 255, 1))
self.slopex.set_pos((0, 0, z))
self.slopex.set_rpy(roll=0, pitch=np.radians(-54.74), yaw=0)
self.slopey = trimesh.primitives.Extrusion(
extrude_polygon=Polygon([np.array([0, 0.100]) * s, np.array([0, 0]), 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 = cm.CollisionModel(self.slopey)
self.slopey.set_rgba((51 / 255, 255 / 255, 255 / 255, 1))
self.slopey.set_pos((0, 0, z))
self.slopey.set_rpy(roll=np.radians(45), pitch=np.radians(90-54.75), yaw=0)
self.slopez = trimesh.primitives.Extrusion(
extrude_polygon=Polygon([np.array([0, -0.100]) * s, np.array([0, 0]), 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.slopez = cm.CollisionModel(self.slopez)
self.slopez.set_rgba((51 / 255, 255 / 255, 255 / 255, 1))
self.slopez.set_pos((0, 0, z))
self.slopez.set_rpy(roll=np.radians(-45), pitch=np.radians(90 - 54.75), yaw=0)
if show:
self.slopex.attach_to(base)
self.slopez.attach_to(base)
self.slopey.attach_to(base)
def reconstruct_surface_list(pcd,
radii=[5],
color=(1, 1, 1),
transparency=1,
toggledebug=False):
pcd = np.asarray(pcd)
tmmeshlist = o3d_helper.reconstructsurfaces_bp(pcd,
radii=radii,
doseparation=True)
obj_list = []
for tmmesh in tmmeshlist:
obj = cm.CollisionModel(initor=tmmesh)
obj_list.append(obj)
if toggledebug:
obj.set_rgba(color[0], color[1], color[2], transparency)
obj.reparentTo(base.render)
return obj_list
def get_obj_from_phoxiinfo_withmodel_nobgf(phxilocator,
stl_f_name,
objpcd_list=None,
phoxi_f_name=None,
load=True,
x_range=(200, 800),
y_range=(-100, 300),
z_range=(790, 1000),
match_rotz=False,
resolution=1,
eps=5,
use_rmse=True):
if stl_f_name[-3:] != 'stl':
stl_f_name += '.stl'
objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT +
'/obstacles/' + stl_f_name))
if objpcd_list is None:
grayimg, depthnparray_float32, pcd = load_phxiinfo(
phoxi_f_name=phoxi_f_name, load=load)
objpcd_list = phxilocator.find_objpcd_list_by_pos(pcd,
x_range=x_range,
y_range=y_range,
z_range=z_range,
toggledebug=False,
scan_num=resolution,
eps=eps)
if len(objpcd_list) == 0:
print("No obj is detected!")
objpcd = phxilocator.find_closest_objpcd_by_stl(stl_f_name,
objpcd_list,
use_rmse=use_rmse)
objmat4 = phxilocator.match_pcdncm(objpcd,
objcm,
toggledebug=False,
match_rotz=match_rotz)
objmat4[:3, :3] = np.eye(3)
objcm.sethomomat(objmat4)
return item.Item(objcm=objcm, pcd=objpcd, objmat4=objmat4)
def __init__(self,
jlobject,
cdprimitive_type='box',
cdmesh_type='triangles'):
"""
author: weiwei
date: 20200331
"""
self.jlobject = jlobject
for id in range(self.jlobject.ndof + 1):
if self.jlobject.lnks[id][
'meshfile'] is not None and self.jlobject.lnks[id][
'collisionmodel'] is None:
# in case the collision model is directly set, it allows manually specifying cd primitives
# instead of auto initialization. Steps: 1. keep meshmodel to None; 2. directly set cm
self.jlobject.lnks[id]['collisionmodel'] = cm.CollisionModel(
self.jlobject.lnks[id]['meshfile'],
cdprimit_type=cdprimitive_type,
cdmesh_type=cdmesh_type)
self.jlobject.lnks[id]['collisionmodel'].set_scale(
self.jlobject.lnks[id]['scale'])
def loadObjpcd(f_name,
pos=(0, 0, 0),
rot=(0, 0, 0),
sample_num=100000,
toggledebug=False):
obj = cm.CollisionModel(
objinit=os.path.join(config.ROOT, "obstacles", f_name))
rotmat4 = np.zeros([4, 4])
rotmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz")
rotmat4[:3, 3] = pos
obj_surface = np.asarray(obj.sample_surface(nsample=sample_num))
# obj_surface = obj_surface[obj_surface[:, 2] > 2]
obj_surface_real = __pcd_trans(obj_surface, rotmat4)
if toggledebug:
# obj_surface = o3d_helper.nparray2o3dpcd(copy.deepcopy(obj_surface))
# obj_surface.paint_uniform_color([1, 0.706, 0])
# o3d.visualization.draw_geometries([obj_surface], window_name='loadObjpcd')
# pcddnp = base.pg.genpointcloudnp(obj_surface_real)
# pcddnp.reparentTo(base.render)
pass
return obj_surface_real
def get_obj_inhand_from_phoxiinfo_withmodel_pcd(phxilocator,
stl_f_name,
tcp_pos,
inithomomat=None,
phoxi_f_name=None,
load=True,
showicp=False,
showcluster=False):
"""
find obj pcd directly by clustering
:param phxilocator:
:param stl_f_name:
:param tcp_pos:
:param inithomomat:
:param phoxi_f_name:
:param temp_f_name:
:param w:
:param h:
:return:
"""
objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT +
'/obstacles/' + stl_f_name))
grayimg, depthnparray_float32, pcd = load_phxiinfo(
phoxi_f_name=phoxi_f_name, load=load)
objpcd = phxilocator.find_objinhand_pcd(tcp_pos,
pcd,
stl_f_name,
toggledebug=showcluster)
objmat4 = phxilocator.match_pcdncm(objpcd,
objcm,
inithomomat,
toggledebug=showicp)
objcm.sethomomat(objmat4)
return item.Item(objcm=objcm,
pcd=objpcd,
objmat4=objmat4,
draw_center=tcp_pos)
def get_org_convexhull(pcd,
color=(1, 1, 1),
transparency=1,
toggledebug=False):
"""
create CollisionModel by pcd
:param pcd:
:param color:
:param transparency:
:return: CollisionModel
"""
convexhull = trimesh.Trimesh(vertices=pcd)
convexhull = convexhull.convex_hull
obj = cm.CollisionModel(initor=convexhull, type="ball")
if toggledebug:
obj.set_rgba(color[0], color[1], color[2], transparency)
obj.reparentTo(base.render)
obj.showlocalframe()
return obj
def find_closest_objpcd_by_stl(self,
src_stl_f_name,
objpcd_list,
inithomomat=None,
use_rmse=True):
objcm = cm.CollisionModel(
objinit=os.path.join(config.ROOT + '/obstacles/' + src_stl_f_name))
min_rmse = 100
max_fitness = 0
result_pcd = None
print("---------------find closest objpcd---------------")
print("find obj:", src_stl_f_name.split(".stl")[0])
for tgt in objpcd_list:
tgt_narray = np.array(tgt)
if inithomomat is None:
inithomomat = self.__match_pos(
np.asarray(ts.sample_surface(objcm.trimesh, count=10000)),
tgt_narray)
src_narray = pcdu.get_objpcd_partial_bycampos(
objcm,
objmat4=inithomomat,
sample_num=len(tgt_narray),
toggledebug=False)
rmse, fitness = self.__get_icp_scores(tgt_narray,
src_narray,
show_icp=False)
if use_rmse:
print("rmse:", rmse, "pcd length", len(tgt))
if 0.0 < rmse < min_rmse:
result_pcd = copy.deepcopy(tgt_narray)
min_rmse = rmse
else:
print("fitness:", fitness, "pcd length", len(tgt))
if fitness != 0.0 and fitness > max_fitness:
result_pcd = copy.deepcopy(tgt_narray)
max_fitness = fitness
return result_pcd
def __init__(self, boundingradius=10.0, betransparent=False):
"""
load obstacles model
separated by category
:param base:
author: weiwei
date: 20181205
"""
self.__this_dir, _ = os.path.split(__file__)
# table
self.__tablepath = os.path.join(self.__this_dir, "../obstacles",
"ur3edtable.stl")
self.__tablecm = cm.CollisionModel(self.__tablepath,
expand_radius=boundingradius,
betransparency=betransparent)
self.__tablecm.setPos(180, 0, 0)
self.__tablecm.setColor(.32, .32, .3, 1.0)
self.__battached = False
self.__changableobslist = []
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 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])
object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2)
obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos,
object_holder_gl_goal_rotmat)
import modeling.geometric_model as gm
import robot_sim.robots.ur3_dual.ur3_dual as ur3d
import robot_sim.robots.ur3e_dual.ur3e_dual as ur3ed
import robot_sim.robots.sda5f.sda5f as sda5
import motion.probabilistic.rrt_connect as rrtc
import pickle
if __name__ == '__main__':
base = wd.World(cam_pos=[2, 1, 3], w=960, h=540, lookat_pos=[0, 0, 1.1])
gm.gen_frame().attach_to(base)
phoxicam = cm.gen_box(extent=np.array([1.000, .43, .1]), homomat=np.eye(4))
phoxicam.set_pos(
np.array([100 + 200, 0, 1360 + 175 + 200 + 90 / 2]) / 1000)
phoxicam.attach_to(base)
object = cm.CollisionModel("./objects/lshape-30.stl")
object.set_scale([0.001, 0.001, 0.001])
object.set_pos(np.array([1.05, -.3, 1.3]))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
component_name = 'rgt_arm'
# robot_instance = ur3d.UR3Dual()
robot_instance = ur3ed.UR3EDual()
# robot_instance = sda5.SDA5F()
# robot_instance.gen_meshmodel().attach_to(base)
with open("experiment180-1123.pickle", "rb") as file:
objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist = pickle.load(
file)
keyposes = []
import math
import numpy as np
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 motion.probabilistic.rrt_connect as rrtc
base = wd.World(cam_pos=[2, 1, 3], lookat_pos=[0, 0, 1.1])
gm.gen_frame().attach_to(base)
# object
object = cm.CollisionModel("objects/bunnysim.stl")
object.set_pos(np.array([.55, -.3, 1.3]))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
# robot_s
component_name = 'lft_arm'
robot_s = ur3d.UR3Dual()
# possible right goal np.array([0, -math.pi/4, 0, math.pi/2, math.pi/2, math.pi / 6])
# possible left goal np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6])
rrtc_planner = rrtc.RRTConnect(robot_s)
path = rrtc_planner.plan(component_name=component_name,
start_conf=robot_s.lft_arm.homeconf,
goal_conf=np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6]),
obstacle_list=[object],
ext_dist=.2,
max_time=300)
print(path)
for pose in path:
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_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl")
dispose_box = cm.CollisionModel(file_dispose_box)
dispose_box.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
dispose_box.set_pos(pos=np.array([.14, 0.07, .003]))
dispose_box.attach_to(base)
rbt_s = cbtr.CobottaRIPPS()
return None, None, None
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
import modeling.collision_model as cm
import grasping.annotation.utils as gutil
import numpy as np
import basis.robot_math as rm
base = wd.World(cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2])
gm.gen_frame().attach_to(base)
objcm = cm.CollisionModel('tubebig.stl')
robot_s = ym.Yumi(enable_cc=True)
manipulator_name = 'rgt_arm'
hand_name = 'rgt_hnd'
start_conf = robot_s.get_jnt_values(manipulator_name)
goal_homomat_list = []
for i in range(6):
goal_pos = np.array([.55, -.1, .3]) - np.array([i * .1, i * .1, 0])
# goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
goal_rotmat = np.eye(3)
goal_homomat_list.append(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm = objcm.copy()
tmp_objcm.set_rgba([1, 0, 0, .3])
tmp_objcm.set_homomat(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm.attach_to(base)
grasp_info_list = gutil.load_pickle_file(objcm_name='tubebig', file_name='yumi_tube_big.pickle')
capsule_1 = capsule_link_start_end(start=np.array([0, 0, 0]),
end=np.array([0, 0, 0.1]))
capsule_2 = capsule_link_start_end(start=np.array([0, 0, 0]),
end=np.array([0, 0.2, 0]))
a_vertices = capsule_1.objtrm.vertices
a_face_normals = capsule_1.objtrm.face_normals
a_vertex_normals = capsule_1.objtrm.face_normals
a_faces = capsule_1.objtrm.faces
capsule_1.objtrm.export("capsule_1.stl")
capsule_2.objtrm.export("capsule_2.stl")
capsule_3 = tb.union([capsule_1.objtrm, capsule_2.objtrm], engine="blender")
capsule_3.export("capsule_3.stl")
capsule_3_wrsmesh = cm.CollisionModel("capsule_3.stl").attach_to(base)
a_sub = trimesh.Trimesh(p).convex_hull
a_cm = cm.CollisionModel(a_sub)
b_sub = trimesh.Trimesh(m).convex_hull
b_cm = cm.CollisionModel(b_sub)
# a_gm = gm.GeometricModel(a_sub).attach_to(base)
print(a_sub)
a_sub.export("a_sub.stl")
b_sub.export("b_sub.stl")
a_trimesh = tri.load("a_sub.stl")
b_trimesh = tri.load("b_sub.stl")
c = tb.union([a_trimesh, b_trimesh], engine="blender")
c.export("c_sub.stl")
# c_wrsmesh = cm.CollisionModel("c_sub.stl").attach_to(base)
import grasping.annotation.utils as gau
if __name__ == '__main__':
import numpy as np
import robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85
import modeling.collision_model as cm
import visualization.panda.world as wd
base = wd.World(cam_pos=[.5, .5, .3], lookat_pos=[0, 0, 0])
gripper_s = rtq85.Robotiq85(enable_cc=True)
objcm = cm.CollisionModel("./objects/bunnysim.stl")
objcm.set_pos(np.array([.5, -.3, 1.2]))
objcm.attach_to(base)
objcm.show_localframe()
grasp_info_list = gau.define_grasp_with_rotation(
gripper_s,
objcm,
gl_jaw_center_pos=np.array([0, 0, 0]),
gl_jaw_center_z=np.array([1, 0, 0]),
gl_hndx=np.array([0, 1, 0]),
jaw_width=.04,
gl_rotation_ax=np.array([0, 0, 1]))
for grasp_info in grasp_info_list:
jaw_width, gl_jaw_center, pos, rotmat = grasp_info
gic = gripper_s.copy()
gic.fix_to(pos, rotmat)
gic.jaw_to(jaw_width)
print(pos, rotmat)
gic.gen_meshmodel().attach_to(base)
base.run()
import modeling.collision_model as cm
import visualization.panda.world as wd
import modeling.geometric_model as gm
import utils
import motion.probabilistic.rrt_connect as rrtc
import motion.trajectory.piecewisepoly_toppra as trajp
if __name__ == '__main__':
base = wd.World(cam_pos=[1.7, .7, .7], lookat_pos=[0, 0, 0])
traj_gen = trajp.PiecewisePolyTOPPRA()
max_vels = [np.pi * .6, np.pi * .4, np.pi, np.pi, np.pi, np.pi * 1.5]
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_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl")
dispose_box = cm.CollisionModel(file_dispose_box, expand_radius=.007)
dispose_box.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
dispose_box.set_pos(pos=np.array([.14, 0.07, .003]))
dispose_box.attach_to(base)
file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl")
import numpy as np
import modeling.geometric_model as gm
import modeling.collision_model as cm
import visualization.panda.world as wd
import basis.robot_math as rm
import math
from scipy.spatial import cKDTree
import vision.depth_camera.surface.rbf_surface as rbfs
base = wd.World(cam_pos=np.array([-.3, -.7, .42]),
lookat_pos=np.array([0, 0, 0]))
# gm.gen_frame().attach_to(base)
bowl_model = cm.CollisionModel(initor="./objects/bowl.stl")
bowl_model.set_rgba([.3, .3, .3, .3])
bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi, 0, 0))
# bowl_model.attach_to(base)
pn_direction = np.array([0, 0, -1])
bowl_samples, bowl_sample_normals = bowl_model.sample_surface(
toggle_option='normals', radius=.002)
selection = bowl_sample_normals.dot(-pn_direction) > .1
bowl_samples = bowl_samples[selection]
bowl_sample_normals = bowl_sample_normals[selection]
tree = cKDTree(bowl_samples)
surface = rbfs.RBFSurface(bowl_samples[:, :2], bowl_samples[:, 2])
surface.get_gometricmodel(rgba=[.3, .3, .3, .3]).attach_to(base)
pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True)
tmp_direction = np.cross(pn_direction, pt_direction)
plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))
