Exemplo n.º 1
0
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]])
Exemplo n.º 2
0
import numpy as np
import reachability

pose_array = np.loadtxt("pose_array.txt")
reachability.get_base_positions(pose_array)