goalrot3 = np.array([[1, 0, 0], [0, -0.92388, -0.382683],
[0, 0.382683, -0.92388]]).T
goalpos3 = goalpos3 - 70.0 * goalrot3[:, 2]
goalpos4 = np.array([454.5617667638, -100, 1150.5])
goalrot4 = np.dot(rm.rodrigues([0, 0, 1], -120), goalrot3)
goalpos4 = goalpos4 - 250.0 * goalrot4[:, 2]
# goalpos2 = goalpos2 - 150.0*goalrot2[:,2]
start = robot.numik(startpos, startrot, armname)
goal3 = robot.numikmsc(goalpos3, goalrot3, msc=start, armname=armname)
print(goal3)
goal4 = robot.numikmsc(goalpos4, goalrot4, msc=start, armname=armname)
print(goal4)
planner = rrtc.RRTConnect(start=goal3,
goal=goal4,
ctcallback=ctcallback,
starttreesamplerate=starttreesamplerate,
endtreesamplerate=endtreesamplerate,
expanddis=5,
maxiter=2000,
maxtime=100.0)
robot.movearmfk(goal3, armname)
robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0)
robotnp.reparentTo(base.render)
robot.movearmfk(goal4, armname)
robotnp = robotmesh.genmnp(robot, jawwidthrgt=85.0, jawwidthlft=0.0)
robotnp.reparentTo(base.render)
# base.run()
robotball.showcn(base, robotball.genfullbcndict(robot))
robot.goinitpose()
[path, sampledpoints] = planner.planning(obscmlist)
path = smoother.pathsmoothing(path, planner)
print(path)
rbt.movearmfk(start_armjnts, "lft")
rbt.movearmfk(rbt.initrgtjnts, "rgt")
# prepare the planner
rbtball = ur3dualball.Ur3DualBall()
cdchecker = cdck.CollisionCheckerBall(rbtball)
ctcallback = ctcb.CtCallback(base,
rbt,
cdchecker=cdchecker,
ctchecker=ctchecker,
ctangle=ctangle,
armname="lft")
planner = rrtc.RRTConnect(start=start_armjnts,
goal=goal1_armjnts,
ctcallback=ctcallback,
starttreesamplerate=30.0,
endtreesamplerate=30.0,
expanddis=10,
maxiter=2000.0,
maxtime=100.0)
smoother = sm.Smoother()
#calculate the object pose relative to the end effector pose
rbtmesh = rbtmg.genmnp(rbt, jawwidthlft=obj.height)
leepos = rbt.lftarm[-1]['linkend']
leerot = rbt.lftarm[-1]['rotmat']
relpos1, relrot1 = rm.relpose(leepos, leerot, startposobj, startrotobj)
#build the tranformation of the object pose in the end effector frame
relpos_t = np.eye(4)
#the rotation part
relpos_t[:3, :3] = relrot1
relpos_t[:3, 3] = relpos1