def delete_demo(demo_type, demo_name):
assert demo_type != '', 'DEMO TYPE needs to be valid.'
assert demo_name != '', 'DEMO NAME needs to be valid.'
demo_type_dir = osp.join(demo_files_dir, demo_type)
demo_dir= osp.join(demo_type_dir, demo_name)
if not osp.exists(demo_dir):
redprint('%s does not exist! Invalid demo.'%demo_dir)
return
yellowprint('This cannot be undone!')
if not yes_or_no('Are you still sure you want to delete %s?'%demo_name):
return
# If it was the last demo recorded, update the latest_demo.txt file
# Otherwise don't bother
latest_demo_file = osp.join(demo_type_dir, latest_demo_name)
with open(latest_demo_file,'r') as fh: demo_num = int(fh.read())
if demo_name == demo_names.base_name%demo_num:
with open(latest_demo_file,'w') as fh: fh.write(str(demo_num-1))
# Delete contents of demo_dir
shutil.rmtree(demo_dir)
# Update master_file
master_file = osp.join(demo_type_dir, master_name)
with open(master_file,"r") as fh: master_lines = fh.readlines()
fh = open(master_file,"w")
for line in master_lines:
if demo_name not in line: fh.write(line)
fh.close()
def load_data(data_file, lr, freq=30.0):
with open(data_file, 'r') as f:
dat = cp.load(f)
demo_dir = osp.dirname(data_file)
cam_types = get_cam_types(demo_dir)
T_cam2hbase = dat['T_cam2hbase']
cam_tmin = float('inf')
cam_info = {}
for kname in dat[lr].keys():
if 'cam' in kname:
if kname != 'camera1': continue
tfs = [tt[0] for tt in dat[lr][kname]]
ts = [tt[1] for tt in dat[lr][kname]]
#ctype_name = int(kname[-1])
## don't append any empty-streams:
if len(ts) > 0:
cam_strm = streamize(tfs, ts, freq, avg_transform)#, tstart=-1./freq)
cam_info[kname] = {'type' : cam_types[kname],
'stream' : cam_strm}
cam_tmin = min(ts[0], cam_tmin)
## hydra data:
hydra_tfs = [tt[0] for tt in dat[lr]['hydra']]
hydra_ts = np.array([tt[1] for tt in dat[lr]['hydra']])
if abs(cam_tmin - hydra_ts[0]) > 1000:
hydra_ts += (cam_tmin - hydra_ts[0])
if len(hydra_ts) <= 0:
redprint("ERROR : No hydra data found in : %s"%(osp.basename(data_file)))
sys.exit(-1)
hydra_strm = streamize(hydra_tfs, hydra_ts, freq, avg_transform)#, tstart=-1./freq)
## potentiometer angles:
pot_vals = np.array([tt[0] for tt in dat[lr]['pot_angles']])
pot_ts = np.array([tt[1] for tt in dat[lr]['pot_angles']])
print cam_tmin, pot_ts[0]
if abs(cam_tmin - pot_ts[0]) > 1000:
pot_ts += (cam_tmin - pot_ts[0])
if len(pot_ts) <= 0:
redprint("ERROR : No potentiometer data found in : %s"%(osp.basename(data_file)))
sys.exit(-1)
pot_strm = streamize(pot_vals, pot_ts, freq, np.mean)#, tstart=-1./freq)
print hydra_ts[0], pot_ts[0]
return (T_cam2hbase, cam_info, hydra_strm, pot_strm)
def align_tf_streams(hydra_strm, cam_strm, wsize=0):
"""
Calculates the time-offset b/w the camera and hydra transform streams.
It uses a hydra-stream because it is full -- it has no missing data-points.
===========================
--> If that is not the case, first fit a spline to the hydra stream and then call this function.
WSIZE : is the window-size to search in :
This function gives the:
argmin_{s \in [-wsize, ..,0,..., wsize]} dist(hydra_stream, cam_stream(t+s))
where, dist is the euclidean norm (l2 norm).
NOTE : (1) It uses only the position variables for distance calculations.
(2) Further assumes that the two streams are on the same time-scale.
"""
Xs_hy = []
Xs_cam = []
cam_inds = []
idx = 0
## pre-empt if the stream is small:
if len(cam_strm.get_data()[0]) <= wsize:
return 0
for tfm in cam_strm:
if tfm != None:
Xs_cam.append(tfm[0,3])
cam_inds.append(idx)
idx += 1
for hydra_tfm in hydra_strm:
if hydra_tfm != None:
Xs_hy.append(hydra_tfm[0,3])
else:
#redprint("None hydra tfm!")
if len(Xs_hy) > 0:
Xs_hy.append(Xs_hy[-1])
## chop-off wsized data from the start and end of the camera-data:
start_idx, end_idx = 0, len(Xs_cam)-1
while cam_inds[start_idx] < wsize and start_idx < len(Xs_cam) : start_idx += 1
while cam_inds[end_idx] >= len(Xs_hy) - wsize and end_idx >= 0: end_idx -= 1
dists = []
cam_inds = np.array(cam_inds[start_idx:end_idx+1])
Xs_cam = np.array(Xs_cam[start_idx:end_idx+1])
Xs_hy = np.array(Xs_hy)
for shift in xrange(-wsize, wsize+1):
dists.append(np.linalg.norm(Xs_hy[shift + cam_inds] - Xs_cam))
shift = xrange(-wsize, wsize+1)[np.argmin(dists)]
redprint("\t stream time-alignment shift is : %d (= %0.3f seconds)"%(shift,hydra_strm.dt*shift))
return shift
def load_data(data_file, lr, freq=30.0, speed=1.0, hydra_only=False):
"""
Changed slightly from the one in demo_data_prep to include speed.
"""
with open(data_file, 'r') as f:
dat = cp.load(f)
demo_dir = osp.dirname(data_file)
with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh)
T_cam2hbase = dat['T_cam2hbase']
if not hydra_only:
cam_info = {}
for kname in dat[lr].keys():
if 'cam' in kname:
tfs = [tt[0] for tt in dat[lr][kname]]
ts = [tt[1] for tt in dat[lr][kname]]
ctype_name = int(kname[-1])
## we will add empty stream
cam_strm = streamize(tfs, ts, freq, avg_transform, speed=speed)#, tstart=-1./freq)
cam_info[ctype_name] = {'type' : cam_types[ctype_name],
'stream' : cam_strm}
## hydra data:
hydra_tfs = [tt[0] for tt in dat[lr]['hydra']]
hydra_ts = np.array([tt[1] for tt in dat[lr]['hydra']])
if len(hydra_ts) <= 0:
redprint("ERROR : No hydra data found in : %s"%(osp.basename(data_file)))
sys.exit(-1)
hydra_strm = streamize(hydra_tfs, hydra_ts, freq, avg_transform, speed=speed)#, tstart=-1./freq)
## potentiometer angles:
pot_vals = np.array([tt[0] for tt in dat[lr]['pot_angles']])
pot_ts = np.array([tt[1] for tt in dat[lr]['pot_angles']])
if len(pot_ts) <= 0:
redprint("ERROR : No potentiometer data found in : %s"%(osp.basename(data_file)))
sys.exit(-1)
pot_strm = streamize(pot_vals, pot_ts, freq, np.mean, speed=speed)#, tstart=-1./freq)
if not hydra_only:
return (T_cam2hbase, cam_info, hydra_strm, pot_strm)
else:
return (T_cam2hbase, hydra_strm, pot_strm)
def segment_streams(ann_dat, strms, time_shifts, demo_dir, base_stream='camera1'):
"""
Segments a list of streams based on the timings in the annotation file
and the time-shift data.
STRMS : A list of streams
TIME_SHIFT : A dictionary of time-shifts applied to the streams wrt to the raw streams
DEMO_DIR : The demo-directory : used to get the annotation file
BASE_STRM : The stream relative to which the annotation times are specified
RETURNS : a list of list of streams. len(list) == number of segments
start_times: the shifted segment start times
"""
if base_stream not in time_shifts.keys():
redprint("Cannot segment streams. Base time shift data is missing for : %s"%base_stream)
n_segs = len(ann_dat)
start_times = np.array([seg_info['look']+time_shifts[base_stream] for seg_info in ann_dat])
stop_times = np.array([seg_info['stop']+time_shifts[base_stream] for seg_info in ann_dat])
nsteps = map(int, (stop_times - start_times) / strms[0].dt)
print start_times
strm_segs = []
for strm in strms:
strm_segs.append(segment_stream(strm, start_times, stop_times))
out_segs = []
for i in xrange(n_segs):
if ann_dat[i]['name'] == 'done':
continue
si = []
for n in xrange(len(strms)):
si.append(strm_segs[n][i])
out_segs.append(si)
return out_segs, nsteps, start_times
if args.prompt_delete and yes_or_no('Do you want to delete %s?'%args.demo_name):
delete_demo(args.demo_type, args.demo_name)
else:
first = args.first
last = args.last
demo_type_dir = osp.join(demo_files_dir, args.demo_type)
if args.last == -1:
latest_demo_file = osp.join(demo_type_dir, latest_demo_name)
if osp.isfile(latest_demo_file):
with open(latest_demo_file,'r') as fh:
last = int(fh.read()) + 1
else:
redprint("No demos!")
for i in xrange (first, last+1):
demo_name = demo_names.base_name%i
demo_dir = osp.join(demo_files_dir, args.demo_type, demo_name)
if osp.exists(demo_dir):
if args.prompt:
raw_input('Hit enter for %s.'%demo_name)
yellowprint("Visualizing: %s"%demo_name)
if args.use_traj:
view_tracking_on_rviz(demo_type=args.demo_type, demo_name=demo_name, tps_model_fname=args.tps_fname,
freq=args.freq, speed=args.speed,
use_smoother=args.use_smoother, prompt=args.prompt, verbose=args.verbose)
else:
if args.hydra_only:
def plan_follow_traj(
robot,
manip_name,
ee_link,
new_hmats,
old_traj,
rope_cloud=None,
rope_constraint_thresh=0.01,
end_pose_constraint=False,
):
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
# init_traj[0] = robot.GetDOFValues(arm_inds)
end_pose = openravepy.poseFromMatrix(new_hmats[-1])
request = {
"basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": False},
"costs": [
{"type": "joint_vel", "params": {"coeffs": [n_steps / 5.0]}},
{"type": "collision", "params": {"coeffs": [1], "dist_pen": [0.01]}},
],
"constraints": [],
"init_info": {"type": "given_traj", "data": [x.tolist() for x in init_traj]},
}
request["costs"] += [
{"type": "collision", "name": "cont_coll", "params": {"coeffs": [50], "dist_pen": [0.05]}},
{"type": "collision", "name": "col", "params": {"continuous": False, "coeffs": [20], "dist_pen": [0.02]}},
]
# impose that the robot goes to final ee tfm at last ts
# the constraint works only when the arm is the 'grasp' arm; otherwise only cost is added
# if end_pose_constraint or not is_fake_motion(new_hmats, 0.1):
# hack to avoid missing grasp
if rope_cloud != None:
closest_point = find_closest_point(rope_cloud, end_pose[4:7])
dist = np.linalg.norm(end_pose[4:7] - closest_point)
if dist > rope_constraint_thresh and dist < 0.05:
end_pose[4:7] = closest_point
redprint("grasp hack is active, dist = %f" % dist)
else:
blueprint("grasp hack is inactive, dist = %f" % dist)
# raw_input()
request["constraints"] += [
{
"type": "pose",
"params": {
"xyz": end_pose[4:7].tolist(),
"wxyz": end_pose[0:4].tolist(),
"link": ee_linkname,
"pos_coeffs": [100, 100, 100],
"rot_coeffs": [10, 10, 10],
},
}
]
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append(
{
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [100, 100, 100],
"rot_coeffs": [10, 10, 10],
},
}
)
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
with saver:
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name,
pos_errs.max(),
pos_errs,
)
return traj
def plan_fullbody(
robot,
env,
new_hmats_by_bodypart,
old_traj_by_bodypart,
end_pose_constraints,
rope_cloud=None,
rope_constraint_thresh=0.01,
allow_base=True,
init_data=None,
):
collision_free = True
for part_name in new_hmats_by_bodypart:
n_steps = len(new_hmats_by_bodypart[part_name])
break
state = np.random.RandomState(None)
init_dofs = []
if "base" in old_traj_by_bodypart.keys():
if "rightarm" in old_traj_by_bodypart.keys():
init_dofs += old_traj_by_bodypart["rightarm"].tolist()
else:
init_dofs += robot.GetDOFValues(robot.GetManipulator("rightarm").GetArmIndices()).tolist()
if "leftarm" in old_traj_by_bodypart.keys():
init_dofs += old_traj_by_bodypart["leftarm"].tolist()
else:
init_dofs += robot.GetDOFValues(robot.GetManipulator("leftarm").GetArmIndices()).tolist()
init_dofs += old_traj_by_bodypart["base"]
else:
arm_dofs = []
if "rightarm" in old_traj_by_bodypart.keys():
arm_dofs = old_traj_by_bodypart["rightarm"]
if "leftarm" in old_traj_by_bodypart.keys():
arm_dofs = np.c_[arm_dofs, old_traj_by_bodypart["leftarm"]]
init_dofs = arm_dofs
if allow_base:
x, y, rot = mat_to_base_pose(robot.GetTransform())
base_dofs = np.c_[x, y, rot]
init_dofs = np.c_[init_dofs, np.repeat(base_dofs, init_dofs.shape[0], axis=0)]
init_dofs = init_dofs.tolist()
bodyparts = new_hmats_by_bodypart.keys()
if "base" in bodyparts: # problem
bodyparts += ["rightarm", "leftarm"]
elif allow_base:
bodyparts.append("base")
costs = []
constraints = []
if "base" in bodyparts:
if old_traj_by_bodypart is None:
raise Exception("Base motion planning must have init_dofs")
n_dofs = len(init_dofs[0])
cost_coeffs = n_dofs * [5]
cost_coeffs[-1] = 500
cost_coeffs[-2] = 500
cost_coeffs[-3] = 500
joint_vel_cost = {"type": "joint_vel", "params": {"coeffs": cost_coeffs}}
costs.append(joint_vel_cost)
else:
joint_vel_cost = {
"type": "joint_vel", # joint-space velocity cost
"params": {"coeffs": [5]}, # a list of length one is automatically expanded to a list of length n_dofs
}
costs.append(joint_vel_cost)
for manip, poses in new_hmats_by_bodypart.items():
if manip == "rightarm":
link = "r_gripper_tool_frame"
elif manip == "leftarm":
link = "l_gripper_tool_frame"
elif manip == "base":
link = "base_footprint"
if manip in ["rightarm", "leftarm"]:
if not end_pose_constraints[manip] and is_fake_motion(poses, 0.1):
continue
if manip in ["rightarm", "leftarm"]:
if end_pose_constraints[manip] or not is_fake_motion(poses, 0.1):
end_pose = openravepy.poseFromMatrix(poses[-1])
if rope_cloud != None:
closest_point = find_closest_point(rope_cloud, end_pose[4:7])
dist = np.linalg.norm(end_pose[4:7] - closest_point)
if dist > rope_constraint_thresh and dist < 0.2:
end_pose[4:7] = closest_point
redprint("grasp hack is active, dist = %f" % dist)
constraints.append(
{
"type": "pose",
"params": {
"xyz": end_pose[4:7].tolist(),
"wxyz": end_pose[0:4].tolist(),
"link": link,
"pos_coeffs": [10, 10, 10],
"rot_coeffs": [10, 10, 10],
},
}
)
poses = [openravepy.poseFromMatrix(hmat) for hmat in poses]
for t, pose in enumerate(poses):
costs.append(
{
"type": "pose",
"params": {
"xyz": pose[4:].tolist(),
"wxyz": pose[:4].tolist(),
"link": link,
"pos_coeffs": [20, 20, 20],
"rot_coeffs": [20, 20, 20],
"timestep": t,
},
}
)
traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, init_dofs, collision_free)
bodypart_traj = traj_to_bodypart_traj(traj, bodyparts)
# Do multi init if base planning
if "base" in bodypart_traj and not allow_base:
waypoint_step = (n_steps - 1) // 2
env_min, env_max = get_environment_limits(env, robot)
current_dofs = robot.GetActiveDOFValues().tolist()
waypoint_dofs = list(current_dofs)
MAX_INITS = 10
n = 0
while True:
collisions = get_traj_collisions(bodypart_traj, robot)
if collisions == []:
break
n += 1
if n > MAX_INITS:
print "Max base multi init limit hit!"
return bodypart_traj, total_cost # return arbitrary traj with collisions
print "Base planning failed! Trying random init. Iteration {} of {}".format(n, MAX_INITS)
# randomly sample x, y within env limits + some padding to accommodate robot
padding = 5.0
env_min_x = env_min[0] - padding
env_min_y = env_min[1] - padding
env_max_x = env_max[0] + padding
env_max_y = env_max[1] + padding
waypoint_x = state.uniform(env_min_x, env_max_x)
waypoint_y = state.uniform(env_min_y, env_max_y)
waypoint_dofs[-3] = waypoint_x
waypoint_dofs[-2] = waypoint_y
init_data = np.empty((n_steps, robot.GetActiveDOF()))
init_data[: waypoint_step + 1] = mu.linspace2d(current_dofs, waypoint_dofs, waypoint_step + 1)
init_data[waypoint_step:] = mu.linspace2d(waypoint_dofs, init_dofs, n_steps - waypoint_step)
traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, collision_free, init_dofs, init_data)
bodypart_traj = traj_to_bodypart_traj(traj, bodyparts)
return bodypart_traj, total_cost
