(-100. / 2., -45. / 2.), (100. / 2., -45. / 2.)])
pose_cov = np.array([[16., 3, 0], [3, 36, 0], [0, 0, 0.04]])
pose_mean = np.array([-5., 4., 0]) # x,y,theta in world frame
pose = np.random.multivariate_normal(pose_mean, pose_cov, 1)[0]
print pose
pose = np.array([[np.cos(pose[2]), -np.sin(pose[2]), pose[0]],
[np.sin(pose[2]), np.cos(pose[2]), pose[1]], [0, 0, 1.]])
num_support_points = 10
pressure_option = 'uniform'
obj = pushedobject.PushedObject('polygon', poly, pose, num_support_points,
pressure_option)
contact_mu = 0.5
motion = motionmodel.MotionModel()
v_push1 = np.array([0., 20.]) # mm/s (in world)
pusher1 = pusher.Pusher(poly=LineString([(-11, -42.5), (11, -42.5)]),
pose=np.eye(3),
v_push=v_push1)
v_push2 = np.array([0., -20.]) # mm/s (in world)
pusher2 = pusher.Pusher(poly=LineString([(-11, 42.5), (11, 42.5)]),
pose=np.eye(3),
v_push=v_push2)
fig = plt.figure(1)
ax = fig.add_subplot(111)
plot_pusher(ax, pusher1.poly)
plot_pusher(ax, pusher2.poly)
# import IPython
poly = Polygon([(63./2.,26./2.),(-63./2.,26./2),(-63./2.,-26./2.),(63./2.,-26./2.)])
pose = [0.,0.,-np.pi/3.] #x,y,theta in world frame
pose = np.array([[np.cos(pose[2]),-np.sin(pose[2]),pose[0]],
[np.sin(pose[2]), np.cos(pose[2]),pose[1]],
[0,0,1]])
num_support_points = 10
pressure_option = 'uniform'
obj = pushedobject.PushedObject('polygon',poly,pose,num_support_points,pressure_option)
contact_mu = 0.5
a,b = (63./2.,26./2)
motion = motionmodel.MotionModel(obj, contact_mu)
v_push_world = np.array([0.,20.]) # mm/s (in world)
pusher_starting_point = list(obj.poly_world.exterior.coords)[3][:2] - np.array([0,-1]) # mm (in world)
IPython.embed()
pusher_line = LineString([pusher_starting_point,pusher_starting_point+1000*v_push_world/np.linalg.norm(v_push_world)])
pusher = (v_push_world, pusher_line)
contact_point_world, contact_normal_world, in_contact = obj.eval_contact(pusher_line,pose,v_push_world)
# solver = ode(object_hand_motion)
# solver.set_integrator('dopri5')
# solver.set_f_params(pusher)
# t0 = 0
# x0 = pose
# solver.set_initial_value(x0,t0)
# t1 = 0.7