def recover(obj_frame_id, global_frame_id, z, slot_pos_obj, reset):
global globalvel
global global_slow_vel
zup = z + 0.03
ori = [0, 0, 1, 0]
center_world = [0.35, 0, 0]
slot_pos_obj = slot_pos_obj + [0]
if reset:
# move above the slot
pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = zup
setCart(pos_recover_probe_world, ori)
# speed down
setSpeed(tcp=global_slow_vel, ori=1000)
# move down to the slot
pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = z
setCart(pos_recover_probe_world, ori)
#pause()
# move to the world center
pos_recover_probe_target_world = center_world
pos_recover_probe_target_world[2] = z
setCart(pos_recover_probe_target_world, ori)
# speed up
setSpeed(tcp=globalvel, ori=1000)
# move to the world center
pos_recover_probe_target_world = center_world
pos_recover_probe_target_world[2] = zup+0.03 # up more to let vicon see the marker
setCart(pos_recover_probe_target_world, ori)
setCart([0.2, 0, z + 0.05], ori) # special
def recover(slot_pos_obj, reset):
global z, zup
#import pdb; pdb.set_trace()
z_recover = 0.016 + z # the height for recovery
z_ofset = z_recover
slot_pos_obj = slot_pos_obj + [0]
_center_world = copy.deepcopy(center_world)
if reset:
# move above the slot
pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = zup
setCart(pos_recover_probe_world, ori)
# speed down
setSpeed(tcp=global_slow_vel, ori=1000)
# move down to the slot
pos_recover_probe_world = coordinateFrameTransform(slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = z_ofset
setCart(pos_recover_probe_world, ori)
#pause()
# move to the world center
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = z_ofset
setCart(pos_recover_probe_target_world, ori)
# speed up
setSpeed(tcp=globalvel, ori=1000)
# move up
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = zup + 0.06 #
setCart(pos_recover_probe_target_world, ori)
def recover(slot_pos_obj, reset):
global z, zup
#import pdb; pdb.set_trace()
z_recover = 0.016 + z # the height for recovery
z_ofset = z_recover
slot_pos_obj = slot_pos_obj + [0]
_center_world = copy.deepcopy(center_world)
if reset:
# move above the slot
pos_recover_probe_world = coordinateFrameTransform(
slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = zup
setCart(pos_recover_probe_world, ori)
# move down a bit in fast speed
pos_recover_probe_world = coordinateFrameTransform(
slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = z_ofset + 0.02
setCart(pos_recover_probe_world, ori)
# move down to the slot
# speed down
setAcc(acc=globalhighacc, deacc=globalacc)
setSpeed(tcp=60, ori=1000)
pos_recover_probe_world[2] = z_ofset
setCart(pos_recover_probe_world, ori)
#pause()
# move to the world center
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = z_ofset
setCart(pos_recover_probe_target_world, ori)
# speed up
setAcc(acc=globalhighacc, deacc=globalhighacc)
setSpeed(tcp=global_highvel, ori=1000)
# move up
pos_recover_probe_target_world = _center_world
pos_recover_probe_target_world[2] = zup + 0.06 #
setCart(pos_recover_probe_target_world, ori)
def recover(obj_frame_id, global_frame_id, z, slot_pos_obj, reset):
global globalvel
global global_slow_vel
zup = z + 0.03
ori = [0, 0, 1, 0]
center_world = [0.35, 0, 0]
slot_pos_obj = slot_pos_obj + [0]
if reset:
# move above the slot
pos_recover_probe_world = coordinateFrameTransform(
slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = zup
setCart(pos_recover_probe_world, ori)
# speed down
setSpeed(tcp=global_slow_vel, ori=1000)
# move down to the slot
pos_recover_probe_world = coordinateFrameTransform(
slot_pos_obj, obj_frame_id, global_frame_id, listener)
pos_recover_probe_world[2] = z
setCart(pos_recover_probe_world, ori)
#pause()
# move to the world center
pos_recover_probe_target_world = center_world
pos_recover_probe_target_world[2] = z
setCart(pos_recover_probe_target_world, ori)
# speed up
setSpeed(tcp=globalvel, ori=1000)
# move to the world center
pos_recover_probe_target_world = center_world
pos_recover_probe_target_world[
2] = zup + 0.03 # up more to let vicon see the marker
setCart(pos_recover_probe_target_world, ori)
setCart([0.2, 0, z + 0.05], ori) # special
setCartRos = rospy.ServiceProxy('/robot2_SetCartesian', robot_SetCartesian)
setZone = rospy.ServiceProxy('/robot2_SetZone', robot_SetZone)
listener = tf.TransformListener()
def xyztolist(pos):
return [pos.x, pos.y, pos.z]
def setCart(pos, ori):
param = (np.array(pos) * 1000).tolist() + ori
#print 'setCart', param
#pause()
setCartRos(*param)
setZone(0)
xs = []
ys = []
zs = []
xt = []
yt = []
zt = []
setSpeed(200, 60)
for x in np.linspace(limits[0], limits[1], 2):
for y in np.linspace(limits[2], limits[3], 2):
for z in np.linspace(limits[4], limits[5], 1):
setCart([x, y, z], ori)
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 run_it(accelerations, speeds, shape, nside, side_params, angles, nrep, shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt):
# hack to restart the script to prevent ros network issues.
global pub
limit = 100
cnt = 0
topics = ['-a']
# enumerate the possible trajectories
for cnt_acc, acc in enumerate(accelerations):
# enumerate the side we want to push
for i in range(nside):
# enumerate the contact point that we want to push
for s in side_params:
if shape_type == 'poly':
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:
for rep in xrange(nrep):
if nrep > 1:
bagfilename = '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)
else:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], 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
if hasRobot:
pos_start_probe_world = coordinateFrameTransformList(pos_start_probe_object, obj_frame_id, global_frame_id, listener)
pos_end_probe_world = coordinateFrameTransformList(pos_end_probe_object, obj_frame_id, global_frame_id, listener)
pos_contact_probe_world = coordinateFrameTransformList(pos_probe_contact_object, obj_frame_id, global_frame_id, listener)
pos_center_obj_world = coordinateFrameTransformList([0,0,0], obj_frame_id, global_frame_id, listener)
else:
pos_start_probe_world = pos_start_probe_object + center_world
pos_end_probe_world = pos_end_probe_object + center_world
pos_contact_probe_world = pos_probe_contact_object + center_world
pos_center_obj_world = center_world
# start bag recording
# move to startPos
setAcc(acc=globalhighacc, deacc=globalhighacc)
setSpeed(tcp=global_highvel, ori=global_highvel)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = zup
setCart(start_pos,ori)
setAcc(acc=globalhighacc, deacc=globalacc)
setSpeed(tcp=global_highvel, ori=1000)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = z
setCart(start_pos,ori)
setSpeed(tcp=global_slow_vel, ori=1000) # some slow speed
mid_pos = copy.deepcopy(pos_contact_probe_world)
mid_pos[2] = z
pub.publish('move_to_mid_pos')
setCart(mid_pos,ori)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
rospy.sleep(0.5)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
if acc == 0: # constant speed
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setSpeed(tcp=speeds[cnt_acc], ori=1000)
else: # there is acceleration
setAcc(acc=acc, deacc=globalmaxacc)
setSpeed(tcp=1000, ori=1000) # some high speed
pub.publish('start_pushing')
setCart(end_pos,ori)
pub.publish('end_pushing')
# end bag recording
helper.terminate_ros_node("/record")
setSpeed(tcp=global_highvel, ori=1000)
setAcc(acc=globalhighacc, deacc=globalhighacc)
# move up vertically
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = zup
setCart(end_pos,ori)
distance_obj_center = np.linalg.norm(np.array(pos_center_obj_world)-np.array(center_world))
# recover
recover(obj_slot, distance_obj_center > allowed_distance)
#pause()
cnt += 1
if cnt > limit: return
def norm(vect):
vect = np.array(vect)
return np.sqrt(np.dot(vect, vect))
setZone(0)
xs = []
ys = []
zs = []
xt = []
yt = []
zt = []
setSpeed(100, 60)
setAcc(acc=globalacc, deacc=globalacc)
for x in np.linspace(limits[0], limits[1], nseg[0]):
for y in np.linspace(limits[2], limits[3], nseg[1]):
for z in np.linspace(limits[4], limits[5], nseg[2]):
# import pdb;pdb.set_trace()
setCart([x, y, z], ori)
js = getJoint()
for th in np.linspace(-180, 180, nrotate):
# import pdb;pdb.set_trace()
setJoint(js.j1, js.j2, js.j3, js.j4, js.j5, th)
# get the marker pos from vicon
vmarkers = ROS_Wait_For_Msg('/vicon/markers', Markers).getmsg()
def main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
# set the parameters
globalvel = 200 # speed for moving around
ori = [0, 0.7071, 0.7071, 0]
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
zup = z + 0.05
probe_radius = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
dist_before_contact = 0.02
dist_after_contact = 0.05
obj_frame_id = '/vicon/SteelBlock/SteelBlock'
global_frame_id = '/map'
dir_save_bagfile = os.environ[
'PNPUSHDATA_BASE'] + '/push_dataset_critical_velocity/'
speeds = np.linspace(40, 200, 10)
shape_db = ShapeDB()
shape_polygon = [[-0.0985 / 2, -0.0985 / 2], [0.0985 / 2, -0.0985 / 2]
] # shape of the objects presented as polygon.
topics = ['/joint_states', '/netft_data', '/tf', '/visualization_marker']
setSpeed(tcp=globalvel, ori=1000)
setZone(0)
make_sure_path_exists(dir_save_bagfile)
# enumerate the speed
for v in speeds:
# enumerate the side we want to push
for i in range(len(shape_polygon) - 1):
# enumerate the contact point that we want to push
for s in [0]:
pos = np.array(shape_polygon[i]) * s + np.array(
shape_polygon[i + 1]) * (1 - s)
pos = np.append(pos, [0])
tangent = np.array(shape_polygon[i + 1]) - np.array(
shape_polygon[i])
normal = np.array([tangent[1], -tangent[0], 0])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
# enumerate the direction in which we want to push
for t in [0]:
bagfilename = 'push_shape=%s_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (
shape_id, v, i, s, t)
bagfilepath = dir_save_bagfile + bagfilename
# if exists then skip it
if 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
# 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_start_probe_world = pos_start_probe_object
pos_start_probe_world[0] += 0.3
pos_end_probe_world = pos_end_probe_object
pos_end_probe_world[0] += 0.3
# 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.1)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
setSpeed(tcp=v, ori=1000)
setCart(end_pos, ori)
setSpeed(tcp=globalvel, ori=1000)
# 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)
setSpeed(tcp=100, ori=30)
# move back to startPos
start_pos = [0.4, 0, z + 0.05]
setCart(start_pos, ori)
def main(argv):
# prepare the proxy, listener
global globalvel, z, zup, z_ofset, 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()
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
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.007 # 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.06
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]
angles = [0]
speeds = []
accelerations = [1]
nside = 1
dist_after_contact = 0.15
dist_after_contact = 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['PNPUSHDATA_BASE'] + '/straight_push/%s/%s/' % (surface_id,shape_id)
else:
dir_save_bagfile = os.environ['PNPUSHDATA_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)
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):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
setSpeed(tcp=100, ori=30)
setZone(0)
# set the parameters
limit = 10000 # number of data points to be collected
ori = [0, 0.7071, 0.7071, 0]
threshold = 0.3 # the threshold force for contact, need to be tuned
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
step_size = 0.0002
obj_des_wrt_vicon = [0,0,-(9.40/2/1000+14.15/2/1000),0,0,0,1]
# visualize the block
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
# 0. move to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos,ori)
start_pos = [0.3, 0.06, z]
setCart(start_pos,ori)
curr_pos = start_pos
# 0.1 zero the ft reading
rospy.sleep(1)
setZero()
rospy.sleep(3)
# 1. move in -y direction till contact -> normal
setSpeed(tcp=30, ori=30)
direc = np.array([0, -step_size, 0])
normal = [0,0,0]
while True:
curr_pos = np.array(curr_pos) + direc
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = ftmsg2list(ROS_Wait_For_Msg('/netft_data', geometry_msgs.msg.WrenchStamped).getmsg())
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener)
# correct box_pose
box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon)))
print 'box_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)
break
#pause()
# 2. use the normal to move along the block
setSpeed(tcp=20, ori=30)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 2.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, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = getAveragedFT()
print index #, '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link', 'vicon/SteelBlock/SteelBlock', listener)
# correct box_pose
box_pose_des_global = mat2poselist( np.dot(poselist2mat(list(box_pos) + list(box_quat)), poselist2mat(obj_des_wrt_vicon)))
#box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
vizBlock(obj_des_wrt_vicon)
br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7], rospy.Time.now(), "SteelBlockDesired", "map")
#print 'box_pos', box_pos, 'box_quat', box_quat
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_radis
contact_pts.append(contact_pt.tolist())
contact_pt[2] = 0.01
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 is in 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
if len(contact_pts) > limit:
break
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=5, ori=30)
setCart(curr_pos + normal * move_away_size, ori)
rospy.sleep(1)
print 'bad ft:', getAveragedFT()
setZero()
rospy.sleep(3)
setCart(curr_pos - normal * overshoot_size, ori)
setSpeed(tcp=20, ori=30)
#all_contact(1:2,:); % x,y of contact position
#all_contact(4:5,:); % x,y contact normal
#all_contact(7:9,:); % box x,y
#all_contact(10:13,:); % box quaternion
#all_contact(14:16,:); % pusher position
# save contact_nm and contact_pt as json file
with open(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.json', 'w') as outfile:
json.dump({'contact_pts': contact_pts, 'contact_nms': contact_nms}, outfile)
# save all_contact as mat file
sio.savemat(os.environ['PNPUSHDATA_BASE']+'/all_contact_real.mat', {'all_contact': all_contact})
setSpeed(tcp=100, ori=30)
# 3. move back to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos,ori)
def main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
# set the parameters
globalvel = 200 # speed for moving around
ori = [0, 0.7071, 0.7071, 0]
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
zup = z + 0.05
probe_radius = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
dist_before_contact = 0.02
dist_after_contact = 0.05
obj_frame_id = '/vicon/SteelBlock/SteelBlock'
global_frame_id = '/map'
dir_save_bagfile = os.environ['PNPUSHDATA_BASE'] + '/push_dataset_critical_velocity/'
speeds = np.linspace(40,200,10)
shape_db = ShapeDB()
shape_polygon = [[-0.0985/2, -0.0985/2], [0.0985/2, -0.0985/2]] # shape of the objects presented as polygon.
topics = ['/joint_states', '/netft_data', '/tf', '/visualization_marker']
setSpeed(tcp=globalvel, ori=1000)
setZone(0)
make_sure_path_exists(dir_save_bagfile)
# enumerate the speed
for v in speeds:
# enumerate the side we want to push
for i in range(len(shape_polygon) - 1):
# enumerate the contact point that we want to push
for s in [0]:
pos = np.array(shape_polygon[i]) *s + np.array(shape_polygon[i+1]) *(1-s)
pos = np.append(pos, [0])
tangent = np.array(shape_polygon[i+1]) - np.array(shape_polygon[i])
normal = np.array([tangent[1], -tangent[0], 0])
normal = normal / norm(normal) # normalize it
normal = np.append(normal, [0])
# enumerate the direction in which we want to push
for t in [0]:
bagfilename = 'push_shape=%s_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (shape_id, v, i, s, t)
bagfilepath = dir_save_bagfile+bagfilename
# if exists then skip it
if 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
# 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_start_probe_world = pos_start_probe_object
pos_start_probe_world[0] += 0.3
pos_end_probe_world = pos_end_probe_object
pos_end_probe_world[0] += 0.3
# 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.1)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
setSpeed(tcp=v, ori=1000)
setCart(end_pos,ori)
setSpeed(tcp=globalvel, ori=1000)
# 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)
setSpeed(tcp=100, ori=30)
# move back to startPos
start_pos = [0.4, 0, z + 0.05]
setCart(start_pos,ori)
def main(argv):
# prepare the proxy, listener
global listener
global vizpub
rospy.init_node('contour_follow', anonymous=True)
listener = tf.TransformListener()
vizpub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
br = TransformBroadcaster()
setSpeed(tcp=100, ori=30)
setZone(0)
# set the parameters
limit = 10000 # number of data points to be collected
ori = [0, 0.7071, 0.7071, 0]
threshold = 0.3 # the threshold force for contact, need to be tuned
z = 0.218 # the height above the table probe1: 0.29 probe2: 0.218
probe_radis = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
step_size = 0.0002
obj_des_wrt_vicon = [
0, 0, -(9.40 / 2 / 1000 + 14.15 / 2 / 1000), 0, 0, 0, 1
]
# visualize the block
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
vizBlock(obj_des_wrt_vicon)
rospy.sleep(0.1)
# 0. move to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos, ori)
start_pos = [0.3, 0.06, z]
setCart(start_pos, ori)
curr_pos = start_pos
# 0.1 zero the ft reading
rospy.sleep(1)
setZero()
rospy.sleep(3)
# 1. move in -y direction till contact -> normal
setSpeed(tcp=30, ori=30)
direc = np.array([0, -step_size, 0])
normal = [0, 0, 0]
while True:
curr_pos = np.array(curr_pos) + direc
setCart(curr_pos, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = ftmsg2list(
ROS_Wait_For_Msg('/netft_data',
geometry_msgs.msg.WrenchStamped).getmsg())
print '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link',
'vicon/SteelBlock/SteelBlock',
listener)
# correct box_pose
box_pose_des_global = mat2poselist(
np.dot(poselist2mat(list(box_pos) + list(box_quat)),
poselist2mat(obj_des_wrt_vicon)))
print 'box_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)
break
#pause()
# 2. use the normal to move along the block
setSpeed(tcp=20, ori=30)
index = 0
contact_pts = []
contact_nms = []
all_contact = []
while True:
# 2.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, ori)
# let the ft reads the force in static, not while pushing
rospy.sleep(0.1)
ft = getAveragedFT()
print index #, '[ft explore]', ft
# get box pose from vicon
(box_pos, box_quat) = lookupTransform('base_link',
'vicon/SteelBlock/SteelBlock',
listener)
# correct box_pose
box_pose_des_global = mat2poselist(
np.dot(poselist2mat(list(box_pos) + list(box_quat)),
poselist2mat(obj_des_wrt_vicon)))
#box_pose_des_global = list(box_pos) + list(box_quat)
#print 'box_pose', box_pose_des_global
vizBlock(obj_des_wrt_vicon)
br.sendTransform(box_pose_des_global[0:3], box_pose_des_global[3:7],
rospy.Time.now(), "SteelBlockDesired", "map")
#print 'box_pos', box_pos, 'box_quat', box_quat
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_radis
contact_pts.append(contact_pt.tolist())
contact_pt[2] = 0.01
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 is in 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
if len(contact_pts) > limit:
break
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=5, ori=30)
setCart(curr_pos + normal * move_away_size, ori)
rospy.sleep(1)
print 'bad ft:', getAveragedFT()
setZero()
rospy.sleep(3)
setCart(curr_pos - normal * overshoot_size, ori)
setSpeed(tcp=20, ori=30)
#all_contact(1:2,:); % x,y of contact position
#all_contact(4:5,:); % x,y contact normal
#all_contact(7:9,:); % box x,y
#all_contact(10:13,:); % box quaternion
#all_contact(14:16,:); % pusher position
# save contact_nm and contact_pt as json file
with open(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.json',
'w') as outfile:
json.dump({
'contact_pts': contact_pts,
'contact_nms': contact_nms
}, outfile)
# save all_contact as mat file
sio.savemat(os.environ['PNPUSHDATA_BASE'] + '/all_contact_real.mat',
{'all_contact': all_contact})
setSpeed(tcp=100, ori=30)
# 3. move back to startPos
start_pos = [0.3, 0.06, z + 0.05]
setCart(start_pos, ori)
setZone = rospy.ServiceProxy('/robot2_SetZone', robot_SetZone)
listener = tf.TransformListener()
def xyztolist(pos):
return [pos.x, pos.y, pos.z]
def setCart(pos, ori):
param = (np.array(pos) * 1000).tolist() + ori
#print 'setCart', param
#pause()
setCartRos(*param)
setZone(0)
xs = []
ys = []
zs = []
xt = []
yt = []
zt = []
setSpeed(200, 60)
for x in np.linspace(limits[0],limits[1], 2):
for y in np.linspace(limits[2],limits[3], 2):
for z in np.linspace(limits[4],limits[5], 1):
setCart([x,y,z], ori)
#pause()
setCartRos(*param)
def norm(vect):
vect = np.array(vect)
return np.sqrt(np.dot(vect, vect))
setZone(0)
xs = []
ys = []
zs = []
xt = []
yt = []
zt = []
setSpeed(100, 60)
setAcc(acc=globalacc, deacc=globalacc)
for x in np.linspace(limits[0],limits[1], nseg[0]):
for y in np.linspace(limits[2],limits[3], nseg[1]):
for z in np.linspace(limits[4],limits[5], nseg[2]):
setCart([x,y,z], ori)
js = getJoint()
for th in np.linspace(-180,180,nrotate):
setJoint(js.j1, js.j2, js.j3, js.j4, js.j5, th)
# get the marker pos from vicon
vmarkers = ROS_Wait_For_Msg('/vicon/markers', Markers).getmsg()
rospy.sleep(0.2)
#pause()
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 run_it(accelerations, speeds, shape, nside, side_params, angles, nrep, shape_type, probe_radius, dir_save_bagfile, dist_after_contact, allowed_distance, opt):
# hack to restart the script to prevent ros network issues.
global pub
limit = 100
cnt = 0
topics = ['-a']
# enumerate the possible trajectories
for cnt_acc, acc in enumerate(accelerations):
# enumerate the side we want to push
for i in range(nside):
# enumerate the contact point that we want to push
for s in side_params:
if shape_type == 'poly':
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:
for rep in xrange(nrep):
if nrep > 1:
bagfilename = '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)
else:
bagfilename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], 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
if hasRobot:
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)
pos_center_obj_world = coordinateFrameTransform([0,0,0], obj_frame_id, global_frame_id, listener)
else:
pos_start_probe_world = pos_start_probe_object + center_world
pos_end_probe_world = pos_end_probe_object + center_world
pos_contact_probe_world = pos_probe_contact_object + center_world
pos_center_obj_world = center_world
# start bag recording
# move to startPos
setAcc(acc=globalhighacc, deacc=globalhighacc)
setSpeed(tcp=global_highvel, ori=global_highvel)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = zup
setCart(start_pos,ori)
setAcc(acc=globalacc, deacc=globalacc)
setSpeed(tcp=globalvel, ori=1000)
start_pos = copy.deepcopy(pos_start_probe_world)
start_pos[2] = z
setCart(start_pos,ori)
setSpeed(tcp=global_slow_vel, ori=1000) # some slow speed
mid_pos = copy.deepcopy(pos_contact_probe_world)
mid_pos[2] = z
pub.publish('move_to_mid_pos')
setCart(mid_pos,ori)
rosbag_proc = helper.start_ros_bag(bagfilename, topics, dir_save_bagfile)
rospy.sleep(0.5)
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = z
if acc == 0: # constant speed
setAcc(acc=globalmaxacc, deacc=globalmaxacc)
setSpeed(tcp=speeds[cnt_acc], ori=1000)
else: # there is acceleration
setAcc(acc=acc, deacc=globalmaxacc)
setSpeed(tcp=1000, ori=1000) # some high speed
pub.publish('start_pushing')
setCart(end_pos,ori)
pub.publish('end_pushing')
# end bag recording
helper.terminate_ros_node("/record")
setSpeed(tcp=global_highvel, ori=1000)
setAcc(acc=globalhighacc, deacc=globalhighacc)
# move up vertically
end_pos = copy.deepcopy(pos_end_probe_world)
end_pos[2] = zup
setCart(end_pos,ori)
distance_obj_center = np.linalg.norm(np.array(pos_center_obj_world)-np.array(center_world))
# recover
recover(obj_slot, distance_obj_center > allowed_distance)
#pause()
cnt += 1
if cnt > limit: return
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;
