def displayResult(x_star, x_star_cov, M_star, toplot_star, x_input, M_input,
toplot_input, x_true, pusher_loc, contact_normal,
contact_point, has_contact, has_apriltag, saveto, titletext,
saveformat, plotconfig, sub):
# 0. convert from matlab col-based to python row-based
if toplot_star:
x_star = make_row_based_npa(x_star, 3)
if toplot_input:
x_input = make_row_based_npa(x_input, 3)
M_star_3d = None
if toplot_star:
if len(M_star.shape) == 2: # time independent
M_star = make_row_based_npa(M_star, 2)
M_star_3d = append_zero_one_horizontally(M_star)
elif len(M_star.shape) == 1: # no input
M_star = make_row_based_npa(M_input, 2)
M_star_3d = append_zero_one_horizontally(M_star)
if toplot_input:
if len(M_input.shape) == 2: # time independent
M_input = npa(M_input)
M_input_3d = append_zero_one_horizontally(M_input)
# 2. load shape
#### add the object as polygon
shape_db = ShapeDB()
shape = shape_db.shape_db[shape_id][
'shape'] # shape of the objects presented as polygon.
shape_type = shape_db.shape_db[shape_id]['shape_type']
if shape_type == 'poly':
shape_polygon_3d = np.hstack((np.array(shape), np.zeros(
(len(shape), 1)), np.ones((len(shape), 1))))
elif shape_type == 'ellip':
shape = shape[0]
elif shape_type == 'polyapprox':
shape_polygon_3d = np.hstack(
(np.array(shape[0]), np.zeros(
(len(shape[0]), 1)), np.ones((len(shape[0]), 1))))
# 3. loop through trajectory and plot the shape
length = len(x_star) if toplot_star else len(x_input)
num_cores = 2
#~ Parallel(n_jobs=num_cores)(delayed(drawit)(i, x_star, x_star_cov, M_star, toplot_star,
#~ x_input, M_input, toplot_input,
#~ x_true, pusher_loc, contact_normal, contact_point, has_contact, has_apriltag,
#~ saveto, titletext, saveformat, plotconfig, sub, M_star_3d, M_input_3d, shape, shape_type, shape_polygon_3d) for i in range(0,length,sub))
for i in xrange(0, length, sub):
drawit(i, x_star, x_star_cov, M_star, toplot_star, x_input, M_input,
toplot_input, x_true, pusher_loc, contact_normal, contact_point,
has_contact, has_apriltag, saveto, titletext, saveformat,
plotconfig, sub, M_star_3d, M_input_3d, shape, shape_type,
shape_polygon_3d)
def animate_2dsynced(data, shape_id, figfname):
fig, ax = plt.subplots()
fig.set_size_inches((7,7))
probe_radius = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
sub = 1 # subsample rate
tip_pose = data['tip_poses_2d']
object_pose = data['object_poses_2d']
force = data['force_2d']
patches = []
# add the object as polygon
shape_db = ShapeDB()
shape_polygon = shape_db.shape_db[shape_id]['shape'] # shape of the objects presented as polygon.
shape_polygon_3d = np.hstack((np.array(shape_polygon), np.zeros((len(shape_polygon), 1)), np.ones((len(shape_polygon), 1))))
print 'object_pose', len(object_pose), 'tip_pose', len(tip_pose), 'force', len(force)
plt.ion()
for i in (range(0, len(tip_pose), sub)):
plt.cla()
T = tfm.compose_matrix(translate = object_pose[i][0:2] + [0], angles = (0,0,object_pose[i][2]) )
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, color='blue', alpha=0.05)
ax.add_patch(obj)
# add the probes as circle
circle = mpatches.Circle(tip_pose[i][0:2], probe_radius, ec="none", color='red', alpha=0.5)
ax.add_patch(circle)
# add the force
ax.arrow(tip_pose[i][0], tip_pose[i][1], force[i][0]/100, force[i][1]/100, head_width=0.005, head_length=0.01, fc='k', ec='k')
#arrow = mpatches.Arrow(tip_pose[i][0], tip_pose[i][1], force[i][0],
# force[i][1], head_width=0.05, head_length=0.1, fc='k', ec='k')
#ax.add_patch(arrow)
# render it
plt.axis([-0.3, 0.3, -0.3, 0.3])
#plt.axis('equal')
plt.draw()
#time.sleep(0.1)
plt.show()
def main(argv):
parser = optparse.OptionParser()
parser.add_option('-s',
action="store",
dest='shape_id',
help='The shape id e.g. rect1-rect3',
default='rect1')
(opt, args) = parser.parse_args()
vicon_ball_size = 0.01415 # in diameter
rospy.init_node('vicon_object_visulizer')
listener = tf.TransformListener()
global vizpub
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
# parameters about object
shape_id = opt.shape_id # should be a rosparam
shape_db = ShapeDB()
mesh = shape_db.shape_db[shape_id]['mesh']
print 'mesh', mesh
frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
thickness = shape_db.shape_db[shape_id]['thickness']
vicon_marker_plate_thick = 0.002
obj_des_wrt_vicon = [
0, 0, -(thickness / 2 + vicon_ball_size / 2 +
vicon_marker_plate_thick + 0.0036), 0, 0, 0, 1
] # from vicon to the block (a slight difference in z)
r = rospy.Rate(100)
while not rospy.is_shutdown():
# get box pose from vicon
try:
vizBlock(obj_des_wrt_vicon, mesh, frame_id)
except:
print 'object not in view'
r.sleep()
def main(argv):
parser = optparse.OptionParser()
parser.add_option('-s', action="store", dest='shape_id',
help='The shape id e.g. rect1-rect3', default='rect1')
(opt, args) = parser.parse_args()
vicon_ball_size = 0.01415 # in diameter
rospy.init_node('vicon_object_visulizer')
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
vizpuboverlay = rospy.Publisher("visualization_marker_overlay", Marker, queue_size=10)
# parameters about object
shape_id = opt.shape_id # should be a rosparam
shape_db = ShapeDB()
mesh = shape_db.shape_db[shape_id]['mesh']
frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
thickness = shape_db.shape_db[shape_id]['thickness']
vicon_marker_plate_thick = 0.002
obj_des_wrt_vicon = [0,0,-(thickness/2 + vicon_ball_size + vicon_marker_plate_thick) ,0,0,0,1] # from vicon to the block (a slight difference in z)
obj_des_wrt_vicon_h = [0,0,-(thickness/2 + vicon_ball_size + vicon_marker_plate_thick)+0.001 ,0,0,0,1] # from vicon to the block (a slight difference in z)
r = rospy.Rate(100)
while not rospy.is_shutdown():
# get box pose from vicon
vizSphere(vizpuboverlay, [0,0,0,0,0,0,1], [0.001,0.001,0.001], 'cross_tip', (0,1,1,1), marker_id=24)
try:
vizBlock(vizpub, obj_des_wrt_vicon, mesh, frame_id)
vizBlock(vizpuboverlay, obj_des_wrt_vicon_h, mesh, 'estimate', (1,0,0,0.9), marker_id=22)
vizBlock(vizpuboverlay, obj_des_wrt_vicon_h, mesh, 'estimate_EKF', (0,1,0,0.9), marker_id=25)
vizBlock(vizpuboverlay, obj_des_wrt_vicon, mesh, 'apriltag', (0,1,1,0.9), marker_id=23)
except:
print 'object not in view'
r.sleep()
def main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('collect_motion_data')
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
parser = optparse.OptionParser()
parser.add_option('-s',
action="store",
dest='shape_id',
help='The shape id e.g. rect1-rect3',
default='rect1')
parser.add_option('',
'--surface',
action="store",
dest='surface_id',
help='The surface id e.g. plywood, abs',
default='plywood')
parser.add_option('-r',
'--real',
action="store_true",
dest='real_exp',
help='Do the real experiment space',
default=False)
parser.add_option('',
'--slow',
action="store_true",
dest='slow',
help='Set slower global speed',
default=False)
(opt, args) = parser.parse_args()
# set the parameters
global globalvel
global global_slow_vel
globalvel = 300 # speed for moving around
globalmaxacc = 100 # some big number means no limit, in m/s^2
globalacc = 1 # some big number means no limit, in m/s^2
global_slow_vel = 30
if opt.slow: globalvel = global_slow_vel
ori = [0, 0, 1, 0]
probe_id = 'probe4'
probe_lengths = {
'probe1': 0.23746,
'probe2': 0.16649,
'probe3': 0.15947,
'probe4': 0.15653
}
probe_length = probe_lengths[
probe_id] # probe1: 0.00626/2 probe2: 0.004745 probe3: 0.00475
ft_length = 0.04703
z = probe_length + ft_length + 0.004 + 0.00 # the height above the table
# parameters about the surface
surface_id = opt.surface_id
surface_thicks = {
'plywood': 0.01158,
'abs': 0.01436,
'silicone_rubber': 0.01436
}
surface_thick = surface_thicks[surface_id] # 0.01158 for plywood
z = z + surface_thick
z_recover = 0.012 + z # the height for recovery probe2: 0.2265 probe 3: 0.2226
zup = z + 0.08 + 0.1 # the prepare and end height
probe_radii = {
'probe1': 0.00626 / 2,
'probe2': 0.004745,
'probe3': 0.00475
}
probe_radius = probe_radii[probe_id]
dist_before_contact = 0.03
dist_after_contact = 0.05
skip_when_exists = True
reset_freq = 1
global_frame_id = '/map'
# parameters about object
shape_id = opt.shape_id
shape_db = ShapeDB()
shape_type = shape_db.shape_db[shape_id]['shape_type']
shape = shape_db.shape_db[shape_id]['shape']
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
# space for the experiment
real_exp = opt.real_exp
if real_exp:
#speeds = reversed([20, 50, 100, 200, 400])
speeds = reversed([-1, 20, 50, 100, 200, 400])
#speeds = reversed([20])
if shape_type == 'poly':
side_params = np.linspace(0, 1, 11)
else:
side_params = np.linspace(0, 1, 40, endpoint=False)
angles = np.linspace(-pi / 180.0 * 80.0, pi / 180 * 80, 9)
else:
speeds = [20, 100, 400, -1]
#speeds = reversed([-1])
#shape = [shape[0]]
side_params = [0.1] #np.linspace(0.1,0.9,3)
angles = [0] #np.linspace(-pi/4, pi/4, 3)
# parameters about rosbag
dir_save_bagfile = os.environ[
'PNPUSHDATA_BASE'] + '/straight_push/%s/push_dataset_motion_full_%s/' % (
surface_id, shape_id)
#topics = ['/joint_states', '/netft_data', '/tf', '/visualization_marker']
topics = ['-a']
setAcc(acc=globalacc, deacc=globalacc)
setSpeed(tcp=globalvel, ori=1000)
setZone(0)
make_sure_path_exists(dir_save_bagfile)
# hack
limit = 100
cnt = 0
# enumerate the speed
for v in speeds:
# enumerate the side we want to push
for i in range(len(shape)):
# enumerate the contact point that we want to push
for s in side_params:
if shape_type == 'poly':
#pos = np.array(shape[i]) *s + np.array(shape[(i+1) % len(shape)]) *(1-s)
pos = np.array(shape[i]) * (1 - s) + np.array(shape[
(i + 1) % len(shape)]) * (s)
pos = np.append(pos, [0])
tangent = np.array(shape[(i + 1) % len(shape)]) - np.array(
shape[i])
normal = np.array([tangent[1], -tangent[0]])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
elif shape_type == 'ellip':
(a, b) = shape[0][0], shape[0][1]
pos = [
shape[0][0] * np.cos(s * 2 * np.pi),
shape[0][1] * np.sin(s * 2 * np.pi), 0
]
normal = [
np.cos(s * 2 * np.pi) / a,
np.sin(s * 2 * np.pi) / b, 0
]
normal = normal / norm(normal) # normalize it
elif shape_type == 'polyapprox':
pos, normal = polyapprox(shape, s)
# enumerate the direction in which we want to push
for t in angles:
bagfilename = 'motion_surface=%s_shape=%s_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (
surface_id, shape_id, v, i, s, t)
bagfilepath = dir_save_bagfile + bagfilename
# if exists then skip it
if skip_when_exists and os.path.isfile(bagfilepath):
print bagfilepath, 'exits', 'skip'
continue
# find the probe pos in contact in object frame
pos_probe_contact_object = pos + normal * probe_radius
# find the start point
direc = np.dot(tfm.euler_matrix(0, 0, t),
normal.tolist() +
[1])[0:3] # in the direction of moving out
pos_start_probe_object = pos_probe_contact_object + direc * dist_before_contact
if shape_type == 'polyapprox' and polyapprox_check_collision(
shape, pos_start_probe_object, probe_radius):
print bagfilename, 'will be in collision', 'skip'
continue
# find the end point
pos_end_probe_object = pos_probe_contact_object - direc * dist_after_contact
# zero force torque sensor
rospy.sleep(0.1)
setZero()
wait_for_ft_calib()
# transform start and end to world frame
pos_start_probe_world = coordinateFrameTransform(
pos_start_probe_object, obj_frame_id, global_frame_id,
listener)
pos_end_probe_world = coordinateFrameTransform(
pos_end_probe_object, obj_frame_id, global_frame_id,
listener)
pos_contact_probe_world = coordinateFrameTransform(
pos_probe_contact_object, obj_frame_id,
global_frame_id, listener)
# start bag recording
# move to startPos
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = zup
setCart(start_pos, ori)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = z
setCart(start_pos, ori)
rosbag_proc = subprocess.Popen(
'rosbag record -q -O %s %s' %
(bagfilename, " ".join(topics)),
shell=True,
cwd=dir_save_bagfile)
print 'rosbag_proc.pid=', rosbag_proc.pid
rospy.sleep(0.5)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
if v > 0: # constant speed
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setSpeed(tcp=v, ori=1000)
setCart(end_pos, ori)
setSpeed(tcp=globalvel, ori=1000)
setAcc(acc=globalacc, deacc=globalacc)
else: # v < 0 acceleration
setSpeed(tcp=30, ori=1000) # some slow speed
mid_pos = copy.deepcopy(pos_contact_probe_world)
mid_pos[2] = z
setCart(mid_pos, ori)
setAcc(acc=-v, deacc=globalmaxacc)
setSpeed(tcp=1000, ori=1000) # some high speed
setCart(end_pos, ori)
setSpeed(tcp=globalvel, ori=1000)
setAcc(acc=globalacc, deacc=globalacc)
# end bag recording
terminate_ros_node("/record")
# move up vertically
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = zup
setCart(end_pos, ori)
# recover
recover(obj_frame_id, global_frame_id, z_recover, obj_slot,
not (cnt % reset_freq))
#pause()
cnt += 1
if cnt > limit:
break
if cnt > limit:
break
if cnt > limit:
break
if cnt > limit:
break
def main(argv):
resolution = 0.0002
shape_db = ShapeDB()
for shape_id, value in shape_db.shape_db.iteritems():
#if not shape_id == 'butter': continue
shape_type = value['shape_type']
if shape_type in ['poly', 'ellip', 'polyapprox']:
# find the boundary limits
shape = value["shape"]
if shape_type == "polyapprox": shape = shape[0]
if shape_type in ['poly']:
xy = zip(*shape)
bounds = [
np.min(xy[0]),
np.max(xy[0]),
np.min(xy[1]),
np.max(xy[1])
]
check_inside = poly_check_inside
elif shape_type in ['polyapprox']:
xy = zip(*shape)
bounds = [
np.min(xy[0]),
np.max(xy[0]),
np.min(xy[1]),
np.max(xy[1])
]
check_inside = polyapprox_check_inside
elif shape_type == 'ellip':
bounds = [-shape[0][0], shape[0][0], -shape[0][1], shape[0][1]]
check_inside = ellip_check_inside
# discretize in the bounds every 0.0001 m
xs = arange(bounds[0], bounds[1], resolution)
ys = arange(bounds[2], bounds[3], resolution)
sumxy = np.array([0, 0])
n_point = 0
# enumerate all the points
# find rc by sum up (x,y) divided by npoints
if shape_type == "polyapprox":
sum_rsq = 0
for x in xs:
for y in ys:
if check_inside(shape, (x, y)):
n_point += 1
sum_rsq += dot(np.array([x, y]), np.array([x, y]))
moment = sum_rsq / n_point * value['mass']
cxy = array([0, 0])
else:
xys = []
for x in xs:
for y in ys:
if check_inside(shape, (x, y)):
sumxy = sumxy + np.array([x, y])
n_point += 1
xys.append((x, y))
cxy = sumxy / n_point
if not ('tri' in shape_id): # then symetric
cxy = array([0, 0])
# sum up (r-rc)^2 in boundary
# times m/npoints in the boundary
sum_rsq = 0
for (x, y) in xys:
sum_rsq += dot(
np.array([x, y]) - cxy,
np.array([x, y]) - cxy)
moment = sum_rsq / n_point * value['mass']
print shape_id, '\n', 'centroid', cxy, 'moment', moment * 1000, 'n_point', n_point
def main(argv):
# prepare the proxy, listener
global global_highvel, globalvel, z, zup, listener, obj_frame_id, obj_slot, pub
pub = rospy.Publisher('/robot_status', String, queue_size=10)
rospy.init_node('collect_motion_data')
listener = tf.TransformListener()
rospy.sleep(1)
parser = optparse.OptionParser()
parser.add_option('-s', action="store", dest='shape_id',
help='The shape id e.g. rect1-rect3', default='rect1')
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('-r', '--real', action="store_true", dest='real_exp',
help='Do the real experiment space',
default=False)
parser.add_option('', '--reptype', action="store", dest='reptype', type='string',
help='Do the real experiment space',
default='normal') # normal: no variation, speed, angle, position
parser.add_option('', '--nrep', action="store", dest='nrep', type='int',
help='Repeat how many times',
default=1)
parser.add_option('', '--slow', action="store_true", dest='slow',
help='Set slower global speed',
default=False)
parser.add_option('', '--probe', action="store", dest='probe_id',
help='The probe id e.g. probe1-5', default='probe5')
(opt, args) = parser.parse_args()
if opt.slow:
globalvel = global_slow_vel;
global_highvel = global_slow_vel;
probe_db = ProbeDB()
probe_length = probe_db.db[opt.probe_id]['length']
# parameters about the surface
surface_id = opt.surface_id
surface_db = SurfaceDB()
surface_thick = surface_db.db[surface_id]['thickness']
z = probe_length + ft_length + surface_thick + 0.009 #- (210-156)/1000.0 # the height above the table
z_recover = 0.012 + z # the height for recovery
zup = z + 0.04 # the prepare and end height
zup = z + 0.08 # the prepare and end height
probe_radius = probe_db.db[opt.probe_id]['radius']
# parameters about object
shape_id = opt.shape_id
shape_db = ShapeDB()
shape_type = shape_db.shape_db[shape_id]['shape_type']
shape = shape_db.shape_db[shape_id]['shape']
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
# space for the experiment
real_exp = opt.real_exp
rep_label = ''
dist_after_contact = 0.05
allowed_distance = 0.04
if real_exp:
if opt.nrep == 1:
accelerations = [0.1, 0.2, 0.5, 0.75, 1, 1.5, 2, 2.5]
speeds = [10, 20, 50, 75, 100, 150, 200, 300, 400, 500]
if shape_type == 'poly':
if shape == 'hex':
side_params = np.linspace(0, 1, 6) # decrease the number of pushes
else:
side_params = np.linspace(0, 1, 11)
else:
side_params = np.linspace(0,1,40,endpoint=False)
angles = np.linspace(-pi/180.0*80.0, pi/180*80, 9)
nside = len(shape)
else:
# set the nominal parameters
side_params = [0.7] #[0.3]#[0.1]#[0.7]
angles = [0]#[0.6981317] #[-0.34906585] #[-0.6981317] ##[0]
speeds = [20]
accelerations = []
nside = 1
dist_after_contact = 0.15
#dist_after_contact = 0.10 #0.05 # hack for video
#shape = shape[0:1] # only do it on one side
# what dimension we want to explore
if opt.reptype == 'normal':
pass
elif opt.reptype == 'angle':
angles = [10, 20, 50]
elif opt.reptype == 'speed':
speeds = [10, 20, 50, 75, 100, 150, 200, 300, 400, 500]
elif opt.reptype == 'position':
side_params = np.linspace(0.5, 1, 6)
allowed_distance = 0
nspeeds = len(speeds)
nacc = len(accelerations)
speeds = np.append(speeds, np.repeat(-1, nacc))
accelerations = np.append(np.repeat(0, nspeeds), accelerations)
else:
accelerations = [0, 0, 0.1, 1, 2.5]
speeds = [50, 400, -1, -1, -1]
angles = np.linspace(-pi/4, pi/4, 2)
if shape_type == 'poly':
side_params = np.linspace(0.1,0.9,1)
else:
side_params = np.linspace(0,1,1,endpoint=False)
# parameters about rosbag
if opt.nrep == 1:
dir_save_bagfile = os.environ['DATA_BASE'] + '/straight_push/%s/%s/' % (surface_id,shape_id)
else:
dir_save_bagfile = os.environ['DATA_BASE'] + '/straight_push_rep/%s/%s/%s/' % (surface_id,shape_id,opt.reptype)
setAcc(acc=globalacc, deacc=globalacc)
setSpeed(tcp=globalvel, ori=1000)
setZone(0)
helper.make_sure_path_exists(dir_save_bagfile)
print side_params
run_it(accelerations, speeds, shape, nside, side_params, angles, opt.nrep,
shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt)
def main(argv):
global lr, vizpub, zup, z, obj_frame_id, probe_radius, step_size
rospy.init_node('contour_follow', anonymous=True)
# prepare the proxy, lr
lr = tf.TransformListener()
br = TransformBroadcaster()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rospy.sleep(0.5)
t = rospy.Time(0)
(opt, args) = parse_args()
# load parameters about the surface
surface_id = opt.surface_id
surface_db = SurfaceDB()
surface_thick = surface_db.db[surface_id]['thickness']
# load parameters about the probe
probe_db = ProbeDB()
probe_id = opt.probe_id
probe_length = probe_db.db["probe5"]['length']
probe_radius = probe_db.db[probe_id]['radius']
# load parameters about the shape
shape_db = ShapeDB()
shape_id = opt.shape_id
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_mesh = shape_db.shape_db[shape_id]['mesh']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
step_size = opt.step_size
z = probe_length + ft_length + surface_thick + 0.009
zup = z + 0.05
# end of preparation
def run_explore(filename):
global obj_init_frame
# 0. check if collected or not
dir_base = data_base + "/multi_pushes/"
jsonfilename = dir_base + '/%s.json' % filename
matfilename = dir_base + '/%s.mat' % filename
if os.path.isfile(jsonfilename):
print "skip", filename
return
# 1. move to startPos
setSpeed(tcp=global_vel)
setZone(0)
start_pos = [center_world[0], center_world[1]] + [zup]
setCart(start_pos)
setAccRos(1, 1)
setZero()
wait_for_ft_calib()
#pause()
import Tkinter
import tkMessageBox
result = tkMessageBox.showinfo("Continue",
"Put the object at the starting place")
# 1.1 get object init pos as new center
box_pose_des_global = lookupTransformList(global_frame_id,
obj_frame_id, lr)
obj_init_frame = pose3d_to_pose2d(box_pose_des_global)
pushlen = 0.12
# 2. Push make the pushes fixed wrt the first frame.
# push: move to the startpose up , move down, push to endpose, move up
# 2.1 push([0, -0.12], [0, -0.12+pushlen])
push_center([0, -0.12], [0, -0.12 + pushlen])
# 2.2 push([0, 0.12], [0, 0.12-pushlen])
push_center([0, 0.12], [0, 0.12 - pushlen])
# 2.3 push([-0.12, -0.08], [-0.12+pushlen, -0.08+pushlen])
push_center([-0.08, -0.12], [-0.12 + pushlen, -0.08 + pushlen])
# 2.4 push([0.08, -0.12], [0.08-pushlen, -0.12+pushlen])
if shape_id == 'rect1':
push_center([0.08, -0.12], [0.08 - pushlen, -0.12 + pushlen])
else:
push_center([0.08, 0.12], [0.08 - pushlen, -0.12 - pushlen])
# 4.1 save contact_nm and contact_pt as json file
helper.make_sure_path_exists(dir_base)
with open(jsonfilename, 'w') as outfile:
json.dump(
{
'all_contact': all_contact,
'isreal': True,
'__title__': colname,
"surface_id": surface_id,
"shape_id": shape_id,
"probe_id": probe_id,
"muc": muc,
"probe_radius": probe_radius,
"offset": offset
},
outfile,
sort_keys=True,
indent=1)
# 4.2 save all_contact as mat file
sio.savemat(matfilename, {'all_contact': all_contact})
#recover(obj_slot, True)
# end of run_explore()
for i in xrange(opt.nrep):
run_explore('multipush_shape=%s_surface=%s_rep=%04d_step=%.4f_april' %
(shape_id, surface_id, i, step_size))
def main(argv):
parser = optparse.OptionParser()
parser.add_option('-s',
action="store",
dest='shape_id',
help='The shape id e.g. rect1-rect3',
default='rect1')
parser.add_option('',
'--surface',
action="store",
dest='surface_id',
help='The surface id e.g. plywood, abs',
default='plywood')
parser.add_option('',
'--nrep',
action="store",
dest='nrep',
type='int',
help='Repeat how many times',
default=2000)
parser.add_option('',
'--reptype',
action="store",
dest='reptype',
type='string',
help='Repeat type',
default='normal')
(opt, args) = parser.parse_args()
if len(args) < 1: # no bagfile name
parser.error(
"Usage: plot_rep_pushes.py [dir_to_rep_push] e.g. /home/mcube/pnpushdata/straight_push_rep"
)
return
dir_to_rep_push = args[0]
vel = 100 #100
acc = 0
i = 0
s = 0.5 #0.1#0.1#0.5 #0.7
t = -0.175 #-0.349#-0.349 #-0.698# #0
figfname_png = dir_to_rep_push + '/rep_push_viz_%s.png' % opt.surface_id
figfname_pdf = dir_to_rep_push + '/rep_push_viz_%s.pdf' % opt.surface_id
figfname_2_png = dir_to_rep_push + '/rep_push_viz_2_%s.png' % opt.surface_id
figfname_2_pdf = dir_to_rep_push + '/rep_push_viz_2_%s.pdf' % opt.surface_id
figfname_hist_png = dir_to_rep_push + '/rep_push_viz_hist_%s.png' % opt.surface_id
figfname_hist_pdf = dir_to_rep_push + '/rep_push_viz_hist_%s.pdf' % opt.surface_id
cachefile = '/tmp/plot_rep_push_%s' % opt.surface_id
import shelve
if os.path.exists(cachefile):
f = shelve.open(cachefile)
vals = f['vals']
#trajs = f['trajs'];
#fts = f['fts']
opt = f['opt']
#trajs_tippose = f['trajs_tippose']
meantraj = f['meantraj']
meantraj_tippose = f['meantraj_tippose']
else:
vals = [] # delta between start and end
trajs = []
trajs_tippose = []
fts = []
for rep in xrange(opt.nrep):
print rep
h5filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.h5' % (
opt.surface_id, opt.shape_id, acc * 1000, vel, i, s, t, rep)
#filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep)
filepath = '%s/%s/%s/%s/%s' % (dir_to_rep_push, opt.surface_id,
opt.shape_id, opt.reptype,
h5filename)
if not os.path.isfile(filepath):
print 'not exists', filepath
continue
f = h5py.File(filepath, "r")
ft_array = f['ft_wrench'].value
object_pose = f['object_pose'].value
tip_pose = f['tip_pose'].value
f.close()
invT0 = np.linalg.inv(
matrix_from_xyzrpy(object_pose[0][1:3].tolist() + [0],
[0, 0, object_pose[0][3]]))
T = matrix_from_xyzrpy(object_pose[-1][1:3].tolist() + [0],
[0, 0, object_pose[-1][3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(T_T0)
vals.append(np.append(trans[0:2], np.unwrap([angles[2]])))
# extract traj
#if rep in xrange(500):
if True:
traj = []
for p in object_pose:
T = matrix_from_xyzrpy(p[1:3].tolist() + [0], [0, 0, p[3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(
T_T0)
traj.append(
np.append([p[0] - object_pose[0][0]],
np.append(trans[0:2],
np.unwrap([angles[2]]))))
trajs.append(traj)
traj_tippose = []
for tip_pose_ in tip_pose:
traj_tippose.append(
np.append([tip_pose_[0] - tip_pose[0][0]],
np.dot(invT0,
tip_pose_[1:3].tolist() + [0, 1])))
trajs_tippose.append(traj_tippose)
def computeMeanTraj(trajs):
lenn = 1000000
for traj in trajs:
lenn = min(lenn, len(traj))
print len(traj)
ncol = len(trajs[0][0])
meantraj = np.zeros((lenn, ncol))
ntraj = len(trajs)
for traj in trajs:
meantraj = meantraj + np.array(traj[0:lenn]) / ntraj
return meantraj
meantraj = computeMeanTraj(trajs)
meantraj_tippose = computeMeanTraj(trajs_tippose)
ll = locals()
'''
shv = shelve.open(cachefile, 'n')
for key, val in ll.iteritems():
try:
shv[key] = val
except:
pass
shv.close()
'''
(x, y, th) = zip(*(vals))
valscov = np.cov(vals, rowvar=0)
valsmean = np.mean(vals, axis=0)
print 'covariance\n', valscov
print 'mean', valsmean
print 'mean', valsmean[0:2] * 1000, 'mm', np.rad2deg(valsmean[2]), 'deg'
eigvs, eigvec = eigsorted(valscov[0:2][:, 0:2])
print 'error_trans:', np.sqrt(eigvs[0] + eigvs[1]) * 1000, 'mm'
print 'error_percent_trans:', np.sqrt(eigvs[0] + eigvs[1]) / np.sqrt(
valsmean[0]**2 + valsmean[1]**2) * 100, '%'
print 'error_rot:', np.rad2deg(np.sqrt(valscov[2][2])), 'deg'
print 'error_percent_rot:', np.sqrt(valscov[2][2]) / np.sqrt(valsmean[2]**
2) * 100, '%'
#from latexify import latexify; latexify(fig_width=3.39, fig_height=3.39*(sqrt(5)-1.0)/2.0*2,scale = 2)
#from latexify import latexify; latexify(scale = 2)
#### add the object as polygon
shape_db = ShapeDB()
shape = shape_db.shape_db[opt.shape_id][
'shape'] # shape of the objects presented as polygon.
shape_type = shape_db.shape_db[opt.shape_id]['shape_type']
if shape_type == 'poly':
shape_polygon_3d = np.hstack((np.array(shape), np.zeros(
(len(shape), 1)), np.ones((len(shape), 1))))
elif shape_type == 'ellip':
shape = shape[0]
elif shape_type == 'polyapprox':
shape_polygon_3d = np.hstack(
(np.array(shape[0]), np.zeros(
(len(shape[0]), 1)), np.ones((len(shape[0]), 1))))
part1 = True
if part1:
f1, ((ax1, ax2)) = plt.subplots(1, 2, sharex=True, sharey=True)
#fig = plt.figure()
plt.sca(ax1)
ax = ax1
(tm, x, y, th) = zip(*meantraj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
ec, fc = 'black', 'orangered'
for ii in np.linspace(0, len(meantraj) - 1, 30):
i = int(ii)
if i == 0:
alpha, fill = (0.3, True)
elif i == len(meantraj) - 1:
alpha, fill = (0.6, True)
else:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid')
ax.add_patch(obj)
#####
###add the probes as circle
probe_radius = 0.00475
for ind, ii in enumerate(np.linspace(0,
len(meantraj_tippose) - 1, 30)):
i = int(ii)
if opt.surface_id == 'abs' and ind < 4: # hack
continue
if i == 0:
alpha, fill = (0.8, False)
elif i == len(meantraj_tippose) - 1:
alpha, fill = (0.8, False)
else:
alpha, fill = (0.8, False)
circle = mpatches.Circle(meantraj_tippose[i][1:3],
probe_radius,
color='black',
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid')
ax.add_patch(circle)
plt.axis('equal')
plt.axis('off')
# ##2. plot all traj
ax = ax2
plt.sca(ax)
for traj in trajs:
(tm, x, y, th) = zip(*traj)
plt.plot(x, y, 'g', alpha=0.5)
# ##plot begin and final mean block
(tm, x, y, th) = zip(*meantraj)
for i in [0, -1]:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid',
zorder=2)
ax.add_patch(obj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
plot_cov_ellipse(valscov[0:2][:, 0:2],
valsmean[0:2],
color='orangered',
fill=True,
alpha=0.9,
zorder=3)
#import pdb; pdb.set_trace()
#plot_cov_ellipse(valscov[0:2][:,0:2], meantraj[-1][1:3], color='orangered', fill=True, alpha=0.9, zorder=3)
plt.axis('equal')
plt.axis('off')
plt.savefig(figfname_png, dpi=200)
plt.savefig(figfname_pdf)
## 3. plot final poses
f2, ((ax3, ax4)) = plt.subplots(1, 2, sharex=False, sharey=False)
ax = ax3
plt.sca(ax)
(xd, yd, thd) = zip(*(vals))
ax.scatter(xd, yd, s=0.2, color='k', alpha=1)
### plot begin and final mean block
ec, fc = 'black', 'orangered'
(tm, x, y, th) = zip(*meantraj)
for i in [0, -1]:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
#obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
#ax.add_patch(obj)
### plot 2 sigma bound
plot_cov_ellipse(valscov[0:2][:, 0:2],
valsmean[0:2],
color='orangered',
fill=True,
alpha=0.9,
zorder=0)
##plot_cov_ellipse(valscov[0:2][:,0:2], valsmean[0:2], 3, color='orangered', fill=True, alpha=0.5, zorder=0)
##ax.add_patch(obj)
ax.set_ylim([0, 1000])
plt.axis('equal')
plt.axis('off')
##ax2.set_title('Scatter plot: $\Delta x$ versus $\Delta y$')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
plt.subplots_adjust(left=0.08,
bottom=0.06,
right=0.97,
top=1.0,
wspace=0.01,
hspace=0.20)
ax = ax4
plt.sca(ax)
## plot begin and final mean block
(tm, x, y, th) = zip(*meantraj)
for i in [0, 1]:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid',
zorder=2)
ax.add_patch(obj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
## plot simulated data
(x_sim, y_sim, th_sim) = zip(*get_sim_data())
line_sim = plt.plot(x_sim, y_sim, '--k')
plt.setp(line_sim, linewidth=2)
T = matrix_from_xyzrpy([x_sim[-1], y_sim[-1], 0], [0, 0, th_sim[-1]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='dashed',
zorder=2)
ax.add_patch(obj)
####
#ax.set_ylim([-0.3,0.05])
plt.axis('equal')
plt.axis('off')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
# ax.set_xlim([-0.2,0.2])
# ax.set_ylim([-0.3,0.05])
plt.savefig(figfname_2_png, dpi=200)
plt.savefig(figfname_2_pdf)
plt.show()
# 5-7 plot histogram
f3, ((ax5, ax6, ax7)) = plt.subplots(1, 3, sharex=False, sharey=False)
plt.sca(ax5)
plt.locator_params(axis='x', nbins=4)
n, bins, patches = ax5.hist(np.array(xd) * 1000,
200,
normed=1,
histtype='stepfilled',
facecolor='none',
label='x',
alpha=1)
ax5.get_yaxis().set_visible(False)
ax5.set_xlabel('$\Delta x$ (mm)')
#ax5.set_title('Histogram of $\Delta x$')
plt.sca(ax6)
plt.locator_params(axis='x', nbins=4)
n, bins, patches = ax6.hist(np.array(yd) * 1000,
200,
normed=1,
histtype='stepfilled',
facecolor='none',
label='y',
alpha=1)
ax6.get_yaxis().set_visible(False)
ax6.set_xlabel('$\Delta y$ (mm)')
#ax6.set_title('Histogram of $\Delta y$')
plt.sca(ax7)
plt.locator_params(axis='x', nbins=4)
n, bins, patches = ax7.hist(np.rad2deg(thd),
200,
normed=1,
histtype='stepfilled',
facecolor='none',
label='theta',
alpha=1)
ax7.get_yaxis().set_visible(False)
ax7.set_xlabel('$\Delta \\theta$ (degree)')
#ax7.set_title('Histogram of $\Delta \\theta$')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
plt.subplots_adjust(left=0.04,
bottom=0.23,
right=0.96,
top=0.87,
wspace=0.22,
hspace=0.20)
plt.savefig(figfname_hist_png, dpi=200)
plt.savefig(figfname_hist_pdf)
plt.show()
def displayResult_pygame(x_star, x_star_cov, x_star_2, x_star_2_cov, M_star,
toplot_star, x_input, M_input, toplot_input, x_true,
pusher_loc, contact_normal, contact_point,
has_contact, has_apriltag, saveto, titletext,
saveformat, plotconfig, sub):
# 0. convert from matlab col-based to python row-based
if toplot_star:
x_star = make_row_based_npa(x_star, 3)
x_star_2 = make_row_based_npa(x_star_2, 3)
if toplot_input:
x_input = make_row_based_npa(x_input, 3)
M_star_3d = None
if toplot_star:
if len(M_star.shape) == 2: # time independent
M_star = make_row_based_npa(M_star, 2)
M_star_3d = append_zero_one_horizontally(M_star)
elif len(M_star.shape) == 1: # no input
M_star = make_row_based_npa(M_input, 2)
M_star_3d = append_zero_one_horizontally(M_star)
if toplot_input:
if len(M_input.shape) == 2: # time independent
M_input = npa(M_input)
M_input_3d = append_zero_one_horizontally(M_input)
# 2. load shape
#### add the object as polygon
shape_db = ShapeDB()
shape = shape_db.shape_db[shape_id][
'shape'] # shape of the objects presented as polygon.
shape_type = shape_db.shape_db[shape_id]['shape_type']
#import pdb; pdb.set_trace()
shape_polygon_3d = None
if shape_type == 'poly':
shape_polygon_3d = np.hstack((np.array(shape), np.zeros(
(len(shape), 1)), np.ones((len(shape), 1))))
elif shape_type == 'ellip':
shape = shape[0]
elif shape_type == 'polyapprox':
shape_polygon_3d = np.hstack(
(np.array(shape[0]), np.zeros(
(len(shape[0]), 1)), np.ones((len(shape[0]), 1))))
# 3. loop through trajectory and plot the shape
length = len(x_star) if toplot_star else len(x_input)
pygame.font.init() # you have to call this at the start,
# if you want to use this module.
myfont = pygame.font.SysFont('Ubuntu', 30)
pygame.init()
WHITE = (255, 255, 255)
size = [1000, 1000]
screen = pygame.display.set_mode(size)
pygame.display.set_caption("isam pose estimation display")
clock = pygame.time.Clock()
#for i in xrange(0,length,sub):
i = 0
#
playmode = False
pygame.key.set_repeat(2, 2)
while True:
screen.fill(LIGHT_YELLOW)
#~ pressed = pygame.key.get_pressed()
#~ if pressed[pygame.K_LEFT] or pressed[pygame.K_a]:
#~ i = (i-sub+length) % length
#~ if pressed[pygame.K_RIGHT]:
#~ i = (i+sub) % length
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
return
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT:
i = (i - sub + length) % length
playmode = False
elif event.key == pygame.K_RIGHT:
i = (i + sub) % length
playmode = False
elif event.key == pygame.K_p:
playmode = True
elif event.key == pygame.K_SPACE:
playmode = False
elif event.key == pygame.K_0:
i = 0
elif event.key == pygame.K_r:
pygame.key.set_repeat(2, 2)
elif event.key == pygame.K_n:
pygame.key.set_repeat()
if playmode:
i = (i + 1) % length
toplot_true = True
drawit_pygame(screen, myfont, i, x_star, x_star_cov, M_star,
toplot_star, x_input, M_input, toplot_input, x_true,
toplot_true, pusher_loc, contact_normal, contact_point,
has_contact, has_apriltag, saveto, titletext, saveformat,
plotconfig, sub, M_star_3d, M_input_3d, shape,
shape_type, shape_polygon_3d, DARK_RED, 0,
'iSAM-estimate')
# toplot_true = False # don't repeat
# toplot_input = False
# drawit_pygame(screen, myfont, i, x_star_2, x_star_2_cov, M_star, toplot_star,
# x_input, M_input, toplot_input,
# x_true, toplot_true, pusher_loc, contact_normal, contact_point, has_contact, has_apriltag,
# saveto, titletext, saveformat, plotconfig, sub, M_star_3d, M_input_3d, shape, shape_type, shape_polygon_3d,
# CYAN, 1, 'EKF-estimate')
pygame.display.flip()
def displayResult(x_star,
x_star_cov,
M_star,
toplot_star,
x_input,
M_input,
toplot_input,
d,
radius,
saveto,
titletext,
saveformat,
plotconfig,
shape_id,
offset,
sub,
turned_norm,
vicon_norm,
label=''):
# 0. convert from matlab col-based to python row-based
if toplot_star:
if len(x_star) == 3:
x_star = npa(x_star).T
else:
x_star = npa(x_star)
if toplot_input:
if len(x_input) == 3:
x_input = npa(x_input).T
else:
x_input = npa(x_input)
if toplot_star:
if len(M_star.shape) == 2:
M_star = npa(M_star).T
M_star_3d = np.hstack((np.array(M_star), np.zeros(
(len(M_star), 1)), np.ones((len(M_star), 1))))
if toplot_input:
if len(M_input.shape) == 2:
M_input = npa(M_input)
M_input_3d = np.hstack(
(np.array(M_input), np.zeros(
(len(M_input), 1)), np.ones((len(M_input), 1))))
# 1. convert groundtruth to list of [x,y,theta]
true_x = []
d = d.T
length = d.shape[0]
for i in xrange(d.shape[0]):
#import pdb; pdb.set_trace()
matlabq = d[i, 10:13].tolist() + [d[i, 9].tolist()]
true_x.append([
d[i][6], d[i][7],
tfm.euler_from_quaternion(matlabq, axes='sxyz')[2]
])
true_x = np.array(true_x)
# 2. load shape
#### add the object as polygon
shape_db = ShapeDB()
shape = shape_db.shape_db[shape_id][
'shape'] # shape of the objects presented as polygon.
shape_type = shape_db.shape_db[shape_id]['shape_type']
if shape_type == 'poly':
shape_polygon_3d = np.hstack((np.array(shape), np.zeros(
(len(shape), 1)), np.ones((len(shape), 1))))
elif shape_type == 'ellip':
shape = shape[0]
elif shape_type == 'polyapprox':
shape_polygon_3d = np.hstack(
(np.array(shape[0]), np.zeros(
(len(shape[0]), 1)), np.ones((len(shape[0]), 1))))
# 3. loop through trajectory and plot the shape
length = len(x_star) if toplot_star else len(x_input)
for i in xrange(0, length, sub):
if i % 100 == 0:
print 'display result', i
fig1 = plt.figure()
ax1 = fig1.add_subplot(111, aspect='equal')
ax1.set_xlim(npa([-0.09, 0.09]) * 1.3 + offset[0])
ax1.set_ylim(npa([-0.09, 0.09]) * 1.3 + offset[1])
has_apriltag = d[i][25] > 0.5
# plot groundtruth shape and pose
T = matrix_from_xyzrpy([true_x[i][0], true_x[i][1], 0],
[0, 0, true_x[i][2]])
if shape_type == 'poly' or shape_type == 'polyapprox':
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
linewidth=2,
linestyle='dashed',
fill=False)
elif shape_type == 'ellip':
scale, shear, angles, trans, persp = tfm.decompose_matrix(T)
obj = mpatches.Ellipse(trans[0:2],
shape[0] * 2,
shape[1] * 2,
angle=angles[2] / np.pi * 180.0,
fill=False,
linewidth=1,
linestyle='solid')
ax1.add_patch(obj)
#fig1.savefig('rect1.png', dpi=200, bbox_inches='tight')
if toplot_star:
if len(M_star.shape) == 3:
M_star_i = npa(M_star[i])
M_star_3d = np.hstack(
(np.array(M_star_i), np.zeros(
(len(M_star_i), 1)), np.ones((len(M_star_i), 1))))
# plot input shape and pose
if toplot_input:
T = matrix_from_xyzrpy([x_input[i][0], x_input[i][1], 0],
[0, 0, x_input[i][2]])
if shape_type == 'poly' or shape_type == 'polyapprox' or shape_type == 'ellip':
M_input_3d_world = np.dot(T, M_input_3d.T)
obj = mpatches.Polygon(M_input_3d_world.T[:, 0:2],
closed=True,
linewidth=1,
linestyle='solid',
fill=False)
ax1.plot(M_input_3d_world.T[:, 0:1],
M_input_3d_world.T[:, 1:2], 'go')
elif shape_type == 'ellip':
scale, shear, angles, trans, persp = tfm.decompose_matrix(T)
obj = mpatches.Ellipse(trans[0:2],
shape[0] * 2,
shape[1] * 2,
angle=angles[2],
fill=False,
linewidth=1,
linestyle='solid')
ax1.add_patch(obj)
# plot estimated shape and pose
if toplot_star:
T = matrix_from_xyzrpy([x_star[i][0], x_star[i][1], 0],
[0, 0, x_star[i][2]])
if shape_type == 'poly' or shape_type == 'polyapprox' or shape_type == 'ellip':
M_star_3d_world = np.dot(T, M_star_3d.T)
obj = mpatches.Polygon(M_star_3d_world.T[:, 0:2],
closed=True,
linewidth=1,
linestyle='solid',
fill=False)
ax1.plot(M_star_3d_world.T[:, 0:1], M_star_3d_world.T[:, 1:2],
'ro')
elif shape_type == 'ellip':
scale, shear, angles, trans, persp = tfm.decompose_matrix(T)
obj = mpatches.Ellipse(trans[0:2],
shape[0] * 2,
shape[1] * 2,
angle=angles[2],
fill=False,
linewidth=1,
linestyle='solid')
ax1.add_patch(obj)
# plot the covariance of pose
if x_star_cov is not None:
plot_cov_ellipse(npa(x_star_cov[i][0:2][:, 0:2]),
npa(x_star[i][0:2]),
ax=ax1,
facecolor=(1, 1, 153 / 255.0, 0.5))
plot_cov_fan(x_star_cov[i][2][2],
npa(x_star[i][0:3]),
npa(true_x[i][0:3]),
ax=ax1)
plot_pos(true_x[i][0:3], ax=ax1)
# no axes
ax1.set_axis_off()
# plot contact point
ax1.plot(d[i, 0], d[i, 1], 'k*')
# plot probe
circle = mpatches.Circle((d[i, 13:15]), radius=radius)
ax1.add_patch(circle)
if not has_apriltag:
ax1.text(offset[0] - 0.1, offset[1] - 0.1, 'No apriltag')
# plot normal
#ax1.arrow(d[i,0], d[i,1], d[i,0]+d[i,3]*0.0001, d[i,1]+d[i,4]*0.0001, head_width=0.01, head_length=0.01, fc='k', ec='k')
ax1.arrow(d[i, 0],
d[i, 1],
d[i, 3] * 0.01,
d[i, 4] * 0.01,
head_width=0.001,
head_length=0.01,
fc='g',
ec='g')
#ax1.arrow(d[i,0], d[i,1], turned_norm[i][0]*0.01, turned_norm[i][1]*0.01, head_width=0.001, head_length=0.01, fc='r', ec='r')
#ax1.arrow(d[i,0], d[i,1], vicon_norm[i][0]*0.01, vicon_norm[i][1]*0.01, head_width=0.001, head_length=0.01, fc='g', ec='g')
fig1.savefig('%s%07d.png' % (saveto, i), dpi=200, bbox_inches='tight')
plt.close(fig1)
def main(argv):
global lr, vizpub, zup, z, obj_frame_id
rospy.init_node('contour_follow', anonymous=True)
# prepare the proxy, lr
lr = tf.TransformListener()
br = TransformBroadcaster()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rospy.sleep(0.5)
t = rospy.Time(0)
#(pos_trasform, ori_trasform) = lr.lookupTransform(global_frame_id, '/link_ft', t)
#(pos_trasform, ori_trasform) = lookupTransform(global_frame_id, '/link_ft', lr)
(opt, args) = parse_args()
# load parameters about the surface
surface_id = opt.surface_id
surface_db = SurfaceDB()
surface_thick = surface_db.db[surface_id]['thickness']
# load parameters about the probe
probe_db = ProbeDB()
probe_id = opt.probe_id
probe_length = probe_db.db["probe5"]['length']
probe_radius = probe_db.db[probe_id]['radius']
# load parameters about the shape
shape_db = ShapeDB()
shape_id = opt.shape_id
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_mesh = shape_db.shape_db[shape_id]['mesh']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
z = probe_length + ft_length + surface_thick + 0.009
zup = z + 0.05
nstart = opt.nstart
if nstart > 1 and nstart % 2 == 1: # correct it if not even nor 1
nstart += 1
def run_explore(filename):
# 0. check if collected or not
dir_base = data_base + "/contour_following/"
jsonfilename = dir_base + '/%s.json' % filename
matfilename = dir_base + '/%s.mat' % filename
if os.path.isfile(jsonfilename):
print "skip", filename
return
# 1. move to startPos
setSpeed(tcp=global_vel)
setZone(0)
start_pos = [center_world[0] + explore_radius, center_world[1]] + [zup]
setCart(start_pos)
# 2. Rough probing
scan_contact_pts = []
# 2.1 populate start poses for small probing in opposite direction
start_configs = []
jmax = (nstart + 1) // 2
kmax = 2 if nstart > 1 else 1
dth = 2 * np.pi / nstart
for j in xrange(jmax):
for k in xrange(kmax):
i = j + k * (nstart // 2)
start_pos = [
center_world[0] + explore_radius * np.cos(i * dth),
center_world[1] + explore_radius * np.sin(i * dth)
]
direc = [-np.cos(i * dth), -np.sin(i * dth), 0]
start_configs.append([start_pos, direc])
print 'direc', direc
# 2.2 move toward center till contact, and record contact points
for i, (start_pos, direc) in enumerate(reversed(start_configs)):
setZero()
wait_for_ft_calib()
setCart(start_pos + [zup])
setCart(start_pos + [z])
curr_pos = start_pos + [z]
cnt = 0
while True:
curr_pos = np.array(curr_pos) + np.array(direc) * step_size
setCart(curr_pos)
# let the ft reads the force in static, not while pushing
rospy.sleep(sleepForFT)
ft = getFTmsglist()
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform(global_frame_id,
obj_frame_id, lr)
box_pose_des_global = list(box_pos) + list(box_quat)
print 'obj_pose', box_pose_des_global
# If in contact, break
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_pt = curr_pos - normal * probe_radius
scan_contact_pts.append(contact_pt.tolist())
if i < len(
start_configs
) - 1: # if the last one, stay in contact and do exploration from there.
setCart([curr_pos[0], curr_pos[1], zup])
break
#if too far and no contact break.
if cnt > explore_radius * 2 / step_size:
break
if len(scan_contact_pts) == 0:
print "Error: Cannot touch the object"
return
# 3. Contour following, use the normal to move along the block
setSpeed(tcp=global_slow_vel)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 3.1 move
direc = np.dot(tfm.euler_matrix(0, 0, 2),
normal.tolist() + [1])[0:3]
curr_pos = np.array(curr_pos) + direc * step_size
setCart(curr_pos)
# 3.2 let the ft reads the force in static, not while pushing
rospy.sleep(sleepForFT)
ft = getAveragedFT()
if index % 50 == 0:
#print index #, '[ft explore]', ft
print index
else:
sys.stdout.write('.')
sys.stdout.flush()
# get box pose from vicon
(box_pos, box_quat) = lookupTransform(global_frame_id,
obj_frame_id, lr)
box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
# 3.3 visualize it.
vizBlock(box_pose_des_global, obj_mesh, global_frame_id)
# 3.4 record the data if in contact
if norm(ft[0:2]) > threshold:
# transform ft data to global frame
ft_global = transformFt2Global(ft)
ft_global[2] = 0 # we don't want noise from z
normal = ft_global[0:3] / norm(ft_global)
contact_nms.append(normal.tolist())
contact_pt = curr_pos - normal * probe_radius
contact_pts.append(contact_pt.tolist())
vizPoint(contact_pt.tolist())
vizArrow(contact_pt.tolist(),
(contact_pt + normal * 0.1).tolist())
# caution: matlab uses the other quaternion order: w x y z.
# Also the normal should point toward the object.
all_contact.append(contact_pt.tolist()[0:2] + [0] +
(-normal).tolist()[0:2] + [0] +
box_pose_des_global[0:3] +
box_pose_des_global[6:7] +
box_pose_des_global[3:6] +
curr_pos.tolist())
index += 1
# 3.5 break if we collect enough
if len(contact_pts) > limit:
break
# 3.6 periodically zero the ft
if len(
contact_pts
) % 500 == 0: # zero the ft offset, move away from block, zero it, then come back
move_away_size = 0.01
overshoot_size = 0 #0.0005
setSpeed(tcp=global_super_slow_vel)
setCart(curr_pos + normal * move_away_size)
rospy.sleep(1)
print 'offset ft:', getAveragedFT()
setZero()
wait_for_ft_calib()
setCart(curr_pos - normal * overshoot_size)
setSpeed(tcp=global_slow_vel)
# 4.1 save contact_nm and contact_pt as json file
helper.make_sure_path_exists(dir_base)
with open(jsonfilename, 'w') as outfile:
json.dump(
{
'all_contact': all_contact,
'scan_contact_pts': scan_contact_pts,
'isreal': True,
'__title__': colname,
"surface_id": surface_id,
"shape_id": shape_id,
"probe_id": probe_id,
"muc": muc,
"probe_radius": probe_radius,
"offset": offset
},
outfile,
sort_keys=True,
indent=1)
# 4.2 save all_contact as mat file
sio.savemat(matfilename, {
'all_contact': all_contact,
'scan_contact_pts': scan_contact_pts
})
# 5. move back to startPos
setSpeed(tcp=global_vel)
start_pos = [curr_pos[0], curr_pos[1]] + [zup]
setCart(start_pos)
recover(obj_slot, True)
# end of run_explore()
for i in xrange(opt.nrep):
run_explore('all_contact_real_shape=%s_rep=%04d' % (shape_id, i))
def main(argv):
global lr, vizpub, zup, z, obj_frame_id, probe_radius, step_size
rospy.init_node('contour_follow', anonymous=True)
# prepare the proxy, lr
lr = tf.TransformListener()
br = TransformBroadcaster()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rospy.sleep(0.5)
t = rospy.Time(0)
(opt, args) = parse_args()
# load parameters about the surface
surface_id = opt.surface_id
surface_db = SurfaceDB()
surface_thick = surface_db.db[surface_id]['thickness']
# load parameters about the probe
probe_db = ProbeDB()
probe_id = opt.probe_id
probe_length = probe_db.db["probe5"]['length']
probe_radius = probe_db.db[probe_id]['radius']
ft_length = probe_db.db["probe5"]['ft_length']
# load parameters about the shape
shape_db = ShapeDB()
shape_id = opt.shape_id
obj_frame_id = shape_db.shape_db[shape_id]['frame_id']
obj_mesh = shape_db.shape_db[shape_id]['mesh']
obj_slot = shape_db.shape_db[shape_id]['slot_pos']
step_size = opt.step_size
z = probe_length + ft_length + surface_thick + 0.013
zup = z + 0.05
# end of preparation
def run_explore(filename):
global obj_init_frame
# 0. check if collected or not
dir_base = data_base + "/multi_pushes_twofinger/"
jsonfilename = dir_base + '/%s.json' % filename
matfilename = dir_base + '/%s.mat' % filename
bagfilename = '%s.bag' % filename
topics = ['-a']
if os.path.isfile(dir_base + bagfilename):
print "skip", filename
return
# 1. move to startPos
setSpeed(tcp=global_vel)
setZone(0)
start_pos = [center_world[0], center_world[1]] + [zup]
#moveGripper(20, 100); # width (mm), speed (mm/s)
setCart(start_pos)
setAccRos(1, 1)
setZero()
wait_for_ft_calib()
#pause()
import Tkinter
import tkMessageBox
result = tkMessageBox.showinfo("Continue",
"Put the object at the starting place")
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_base)
rospy.sleep(0.5)
# 1.1 get object init pos as new center
box_pose_des_global = lookupTransformList(global_frame_id,
obj_frame_id, lr)
obj_init_frame = pose3d_to_pose2d(box_pose_des_global)
#obj_init_frame = [0.35, -0.035, 0.0] # hack
pushlen = 0.12
blocklen = 0.045
first_push_goal = 0
if shape_id == "ellip2":
pushlen += 0.01
if shape_id == "butter":
pushlen += 0.0
first_push_goal = 0.04
#return
# 2. Push make the pushes fixed wrt the first frame.
# push: move to the startpose up , move down, push to endpose, move up
# 2.1
push_center([0, -pushlen, np.pi / 2], [0, -first_push_goal, np.pi / 2])
# 2.2
push_center([0, pushlen, np.pi / 2], [0, blocklen, np.pi / 2])
# 2.3
push_center([pushlen, 0, 0], [0, 0, 0])
# 2.4
push_center([-pushlen, 0, 0], [-blocklen, 0, 0])
# 2.5
push_center([-pushlen, -pushlen, np.pi / 4],
[blocklen, blocklen, np.pi / 4])
# 2.6
push_center([pushlen, pushlen, np.pi / 4],
[blocklen, blocklen, np.pi / 4])
# 2.7
push_center([0, pushlen, np.pi / 4], [0, blocklen / 2, np.pi / 4])
# 2.8
push_center([0, -pushlen, np.pi / 4], [0, -blocklen, np.pi / 4])
helper.terminate_ros_node("/record")
# end of run_explore()
for i in xrange(opt.nrep):
run_explore('multipush_shape=%s_surface=%s_rep=%04d_step=%.4f_april' %
(shape_id, surface_id, i, step_size))