import reachability as reach
import numpy as np
f = np.load("/home/joschu/bulletsim/data/knots/l_reachability.npz")
z = .7
y = 0 #left to right
xs = np.linspace(0,1,50)
xyzs = np.array([(x,y,z) for x in xs])
reach.init(True)
base_xys = reach.get_base_positions(xyzs)
base_xyzs = np.c_[base_xys, np.zeros((len(base_xys),1))]
handles = []
handles.append(reach.ENV.drawlinestrip(points=xyzs,
linewidth=3.0))
handles.append(reach.ENV.drawlinestrip(points=base_xyzs,
linewidth=3.0))
for i in xrange(len(base_xys)):
base_tf = np.eye(4)
base_tf[0:2,3] = base_xys[i]
xarm, yarm, zarm = xyzs[i]
arm_tf = np.array([[-1, 0, 0, xarm],
[0, 0, 1, yarm],
[0, 1, 0, zarm],
[0, 0, 0, 1]])
import numpy as np
import reachability
pose_array = np.loadtxt("pose_array.txt")
reachability.get_base_positions(pose_array)