def iscollided(self, xyzrpy, objcm, obstaclecmlist):
"""
:param object:
:param obstaclecdnplist:
:return:
author: weiwei, hao
date: 20190429
"""
objcm.setPos(xyzrpy[0],xyzrpy[1],xyzrpy[2])
objcm.setRPY(xyzrpy[3], xyzrpy[4], xyzrpy[5])
checker = bcdh.MCMchecker()
result = checker.isMeshMeshListCollided(objcm, obstaclecmlist)
# if type(contactpoints) is tuple:
# print(contactpoints[0])
# a = contactpoints[0].getManifoldPoint()
# print(a)
# b = a.getDistance()
# print(b)
# else:
# print("listlistlist")
# print(len(contactpoints))
# print(type(contactpoints))
# for i in range(len(contactpoints)):
# print(di(contactpoints[i]))
return result
def __init__(self, startworld=True, autorotate=False):
"""
helper function to simplify everything
:return:
"""
self.env = yumisetting.Env()
self.obscmlist = self.env.getstationaryobslist()
self.rgthndfa = yi.YumiIntegratedFactory()
self.lfthndfa = self.rgthndfa
self.rgthnd = self.rgthndfa.genHand()
self.lfthnd = self.lfthndfa.genHand()
self.rbt = yumi.YumiRobot(self.rgthnd, self.lfthnd)
self.rbtball = yumiball.YumiBall()
self.rbtmesh = yumimesh.YumiMesh()
self.pcdchecker = cdck.CollisionCheckerBall(self.rbtball)
self.bcdchecker = bch.MCMchecker(toggledebug=False)
self.ctcallback = ctcb.CtCallback(self.rbt,
self.pcdchecker,
armname="rgt")
self.smoother = sm.Smoother()
self.root = os.path.abspath(os.path.dirname(__file__))
self.p3dh = p3dh
self.cm = cm
self.np = np
self.rm = rm
if startworld:
self.base = self.startworld(autorotate=autorotate)
def iscollided(self, xyzrpy, objcm, obstaclecmlist):
"""
:param object:
:param obstaclecdnplist:
:return:
author: weiwei, hao
date: 20190429
"""
objcm.setPos(xyzrpy[0], xyzrpy[1], xyzrpy[2])
objcm.setRPY(xyzrpy[3], xyzrpy[4], xyzrpy[5])
checker = bcdh.MCMchecker()
result = checker.isMeshMeshListCollided(objcm, obstaclecmlist)
if result:
return True
return False
def __init__(self, obj, rhx, retractdistance=100):
"""
:param obj: obj name (str) or objcm, objcm is for debug purpose
:param rhx: see helper.py
:param retractdistance: retraction distance
author: hao, refactored by weiwei
date: 20191206, 20200104osaka
"""
if isinstance(obj, str):
self.objname = obj
elif isinstance(obj, rhx.cm.CollisionModel):
self.objname = obj.name
self.objcm = obj
self.rhx = rhx
self.rbt = rhx.robot_s
self.retractdistance = retractdistance
self.bcdchecker = bch.MCMchecker(toggledebug=False)
with open(
os.path.join(rhx.root, "grasp" + rhx.rgthndfa.name,
"predefinedgrasps.pickle"), "rb") as file:
graspdata = pickle.load(file)
self.identityglist_rgt = graspdata[self.objname]
with open(
os.path.join(rhx.root, "grasp" + rhx.lfthndfa.name,
"predefinedgrasps.pickle"), "rb") as file:
graspdata = pickle.load(file)
self.identityglist_lft = graspdata[self.objname]
self.rgthndfa = rhx.rgthndfa
self.lfthndfa = rhx.lfthndfa
# paramters
self.fpsnpmat4 = []
self.identitygplist = [] # grasp pair list at the identity pose
self.fpsnestedglist_rgt = {
} # fpsnestedglist_rgt[fpid] = [g0, g1, ...], fpsnestedglist means glist at each floating pose
self.fpsnestedglist_lft = {
} # fpsnestedglist_lft[fpid] = [g0, g1, ...]
self.ikfid_fpsnestedglist_rgt = {} # fid - feasible id
self.ikfid_fpsnestedglist_lft = {}
self.ikjnts_fpsnestedglist_rgt = {}
self.ikjnts_fpsnestedglist_lft = {}
from panda3d.core import Vec4
import numpy as np
import database.dbaccess as db
import random
import trimesh as tm
import time
import utiltools.robotmath as rm
import matplotlib.pyplot as plt
import environment.bulletcdhelper as bh
import pickle
import os
# base = pandactrl.World(camp=[700,300,700], lookatp=[0,0,100])
base = pandactrl.World(camp=[700,-300,300], lookatp=[0,0,100])
mc = bh.MCMchecker()
# objpath = "../objects/sandpart.stl"
# objpath = "../objects/043screwdriver1000.stl"
# objpath = "../objects/035drill1000.stl"
# objpath = "../objects/043screwdriver1000.stl"
# objpath = "../objects/025mug1000.stl"
# objpath = "../objects/025mug3000.stl"
# objpath = "../objects/025mug3000_tsdf.stl"
# objpath = "../objects/035drill3000_tsdf.stl"
# objpath = "../objects/072toyplanedrill3000_tsdf.stl"
# objpath = "../objects/housing.stl"
# objpath = "../objects/ttube.stl"
# objpath = "./objects/bunny12356.stl"
# objstpath = "./objects/bunny17432.stl"
pathname = "./objects/"
_this_dir, _ = os.path.split(__file__)
objpath = os.path.join(_this_dir, "objects", "tubelarge.stl")
objcm = cm.CollisionModel(objinit=objpath)
armname = "rgt"
hndfa = yi.YumiIntegratedFactory()
hndfa = yi.YumiIntegratedFactory()
rgthnd = hndfa.genHand()
lfthnd = hndfa.genHand()
robot = yumi.YumiRobot(rgthnd, lfthnd)
robotball = yumiball.YumiBall()
robotmesh = yumimesh.YumiMesh()
pcdchecker = cdball.CollisionCheckerBall(robotball)
bcdchecker = bch.MCMchecker(toggledebug=True)
# robot_s.opengripper(arm_name=arm_name)
robotnp = robotmesh.genmnp(robot)
# robotnp.reparentTo(base.render)
gdb = db.GraspDB(database="yumi")
print("Loading from database...")
fgdata = freegrip.FreegripDB(gdb, objcm.name, rgthnd.name)
tubecmlist = []
for x in range(250, 350, 35):
for y in range(-200, -100, 35):
objlttemp = copy.deepcopy(objcm)
objlttemp.setPos(x, y, 0)
tubecmlist.append(objlttemp)
np.array([[posL[0][0], posL[0][1], posL[0][2]],
[posL[1][0], posL[1][1], posL[1][2]],
[posL[2][0], posL[2][1], posL[2][2]]])))
print(virtualgoalrot0[0], virtualgoalrot0[1], virtualgoalrot0[2])
# objcm.setPos(-63.11388425, 0, 44.4996245)
# objcm.setRPY(0,15,0)
objcm.setPos(-60, 0, 40.5)
objcm.setRPY(0, 14, 0)
# objcm.setPos(-45.2, 0, 30.2)
# objcm.setRPY(0, 0, 0)
objcm.setColor(1, 1, 0, 0.7)
objcm.reparentTo(base.render)
obscmlist = []
obscmlist.append(groove)
checker = bcdh.MCMchecker()
result = checker.isMeshMeshListCollided(objcm, obscmlist)
print(result)
# # planner = ddrrtc.DDRRTConnect()
# ctcallback = CtCallback()
# start = np.array([virtualgoalpos0[0], virtualgoalpos0[1], virtualgoalpos0[2],
# virtualgoalrot0[0], virtualgoalrot0[1], virtualgoalrot0[2]], dtype=float)
# print(start)
# goal = np.array([-45, 0, 30, 0, 0, 0], dtype=float)
# print(goal)
# planner = ddrrtc.DDRRTConnect(start=start, goal=goal, ctcallback=ctcallback,
# starttreesamplerate=30, endtreesamplerate=30,
# expanddis=.5, maxiter=5000, maxtime=200.0)
#
# obscmlist = []
import copy
import pickle
import numpy as np
import environment.bulletcdhelper as bch
import environment.collisionmodel as cm
import utiltools.robotmath as rm
bcdchecker = bch.MCMchecker(toggledebug=False)
def ishndobjcollided(hndfa, jawwidth, homomat, objcm):
"""
:param homomat:
:param obstaclecmlist:
:return:
author: ruishuang, revised by weiwei
date: 20191122
"""
hnd = hndfa.genHand()
hnd.set_homomat(homomat)
setjawwidth = 30 if jawwidth>=30 else jawwidth+10
hnd.setjawwidth(setjawwidth)
iscollided = bcdchecker.isMeshListMeshListCollided(hnd.cmlist, [objcm])
return iscollided
def define_grasp(hndfa, finger_center, finger_normal, hand_normal, jawwidth, objcm, toggleflip = True):
"""
raise ValueError("File or data not found!")
if __name__ == '__main__':
import manipulation.grip.freegrip as fg
import environment.bulletcdhelper as bcd
base = pc.World(camp=[2000, -2000, 1500],
lookatpos=[0, 0, 100],
up=[0, -1, 1],
autocamrotate=False)
hndfa = yi.YumiIntegratedFactory()
objcm = cm.CollisionModel(objinit="../objects/" + "vacuumhead.stl")
gp = fg.Freegrip(objinit="../objects/" + "vacuumhead.stl",
hand=hndfa.genHand())
bmc = bcd.MCMchecker(toggledebug=True)
gp.segShow(base,
togglesamples=False,
togglenormals=False,
togglesamples_ref=False,
togglenormals_ref=False,
togglesamples_refcls=False,
togglenormals_refcls=False,
alpha=1)
predefinedgrasps = load_pickle_file_grip(objcm.name)
counter = [0]
hndnps = [None]
def update(predefinedgrasps, counter, hndnps, bmc, objcm, task):