def planmotionmultiplehold(self, initjnts, goaljnts, objcmlist,
objrelmatlist, obscmlist, armname):
"""
:param initjnts:
:param goaljnts:
:param objcmlist:
:param objrelmatlist: [[pos, rot], [pos, rot], ...]
:param armname:
:param obscmlist: objcmlist including those in the env
:return:
author: weiwei
date: 20191229osaka
"""
self.ctcallback.setarmname(armname=armname)
starttreesamplerate = 30.0
endtreesamplerate = 30.0
planner = rrtc.RRTConnect(start=initjnts,
goal=goaljnts,
ctcallback=self.ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=10,
maxiter=400,
maxtime=15.0)
path, _ = planner.planninghold(objcmlist, objrelmatlist, obscmlist)
if path is None:
print("No path found in planning hold!")
path = self.smoother.pathsmoothinghold(path, planner, maxiter=100)
return path
def planmotion(self, initjnts, goaljnts, obscmlist, armname, expanddis=30):
"""
:param initjnts:
:param goaljnts:
:param armname:
:param obscmlist: objcmlist including those in the env
:param expanddis
:return:
author: weiwei
date: 20191229osaka
"""
self.ctcallback.setarmname(armname=armname)
starttreesamplerate = 30.0
endtreesamplerate = 30.0
planner = rrtc.RRTConnect(start=initjnts,
goal=goaljnts,
ctcallback=self.ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=expanddis,
maxiter=400,
maxtime=15.0)
path, _ = planner.planning(obscmlist)
if path is None:
print("No path found in planning!")
return None
path = self.smoother.pathsmoothing(path, planner, maxiter=50)
return path
def planmotionhold(self,
initjnts,
goaljnts,
objcm,
relpos,
relrot,
obscmlist,
armname,
expanddis=15):
"""
:param initjnts:
:param goaljnts:
:param objcm:
:param relpos:
:param relrot:
:param armname:
:param obscmlist: objcmlist including those in the env
:return:
author: weiwei
date: 20191229osaka
"""
self.ctcallback.setarmname(armname=armname)
starttreesamplerate = 30.0
endtreesamplerate = 30.0
planner = rrtc.RRTConnect(start=initjnts,
goal=goaljnts,
ctcallback=self.ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=expanddis,
maxiter=400,
maxtime=15.0)
path, _ = planner.planninghold([objcm], [[relpos, relrot]], obscmlist)
if path is None:
self.rbt.movearmfk(initjnts, armname)
self.rbtmesh.genmnp(self.rbt).reparentTo(base.render)
abpos, abrot = self.rbt.getworldpose(relpos, relrot, armname)
objcm.set_homomat(self.rm.homobuild(abpos, abrot))
objcm.reparentTo(base.render)
objcm.showcn()
for obscm in obscmlist:
obscm.reparentTo(base.render)
obscm.showcn()
self.rbt.movearmfk(goaljnts, armname)
self.rbtmesh.genmnp(self.rbt).reparentTo(base.render)
abpos, abrot = self.rbt.getworldpose(relpos, relrot, armname)
objcmcopy = copy.deepcopy(objcm)
objcmcopy.set_homomat(self.rm.homobuild(abpos, abrot))
objcmcopy.reparentTo(base.render)
objcmcopy.showcn()
for obscm in obscmlist:
obscmcopy = copy.deepcopy(obscm)
obscmcopy.reparentTo(base.render)
obscmcopy.showcn()
base.run()
print("No path found in planning hold!")
path = self.smoother.pathsmoothinghold(path, planner, maxiter=200)
return path
starttreesamplerate = 50
endtreesamplerate = 50
rbtstartpos = np.array([250, -250, 200])
rbtstartrot = np.array([[1, 0, 0], [0, -0.92388, -0.382683],
[0, 0.382683, -0.92388]]).T
# start = robot_s.numik(rbtstartpos, rbtstartrot, arm_name=arm_name)
# print(start)
rbtgoalpos = np.array([300, -200, 200])
rbtgoalrot = np.dot(rm.rodrigues([0, 0, 1], 90), rbtstartrot)
goal = robot.numik(rbtgoalpos, rbtgoalrot, armname=armname)
print(goal)
planner = rrtc.RRTConnect(start=start,
goal=goal,
ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=30,
maxiter=2000,
maxtime=100.0)
robot.movearmfk(start, armname)
robotnp = robotmesh.genmnp(robot)
robotnp.reparentTo(base.render)
robot.movearmfk(goal, armname)
robotnp = robotmesh.genmnp(robot)
robotnp.reparentTo(base.render)
robotball.showcn(robotball.genfullbcndict(robot))
[path, sampledpoints] = planner.planning(obscmlist + tubecmlist)
path = smoother.pathsmoothing(path, planner, maxiter=100)
print(path)
for pose in path: