def test_translation(demo_name, exp_name, data_dir):
translation_test_init()
verb_data_accessor = multi_item_verbs.VerbDataAccessor(test_info_dir=osp.join("test", TEST_DATA_DIR, data_dir))
current_stage = 1
# info and data for tool stage
demo_tool_info = verb_data_accessor.get_stage_info(demo_name, current_stage-1)
demo_tool_data = verb_data_accessor.get_demo_stage_data(demo_tool_info.stage_name)
exp_tool_info = verb_data_accessor.get_stage_info(exp_name, current_stage-1)
exp_tool_data = verb_data_accessor.get_demo_stage_data(exp_tool_info.stage_name)
# info and data for target stage
demo_target_info = verb_data_accessor.get_stage_info(demo_name, current_stage)
demo_target_data = verb_data_accessor.get_demo_stage_data(demo_target_info.stage_name)
exp_target_info = verb_data_accessor.get_stage_info(exp_name, current_stage)
exp_target_data = verb_data_accessor.get_demo_stage_data(exp_target_info.stage_name)
gripper_data_key = "%s_gripper_tool_frame" % demo_target_info.arms_used
# point clouds of tool for demo and experiment
exp_tool_pc = exp_tool_data["object_cloud"][exp_tool_info.item]["xyz"]
exp_target_pc = exp_target_data["object_cloud"][exp_target_info.item]["xyz"]
# calculate the transformation from the world frame to the gripper frame in the experiment scene
world_to_grip_transform = get_world_to_grip_exp_transform(exp_tool_data, gripper_data_key)
world_to_grip_transform_func = multi_item_make_verb_traj.make_world_to_grip_transform_hmat(world_to_grip_transform)
warped_traj_resp = multi_item_make_verb_traj.make_traj_multi_stage_do_work(demo_name, exp_target_pc,
None, current_stage,
demo_tool_info, exp_tool_pc,
verb_data_accessor, world_to_grip_transform_func,
"tps")
# assuming that the arms_used for the target stage is 'l' or 'r'
if demo_target_info.arms_used == 'l':
warped_grip_traj_mats = [juc.pose_to_hmat(pose) for pose in warped_traj_resp.traj.l_gripper_poses.poses]
elif demo_target_info.arms_used == 'r':
warped_grip_traj_mats = [juc.pose_to_hmat(pose) for pose in warped_traj_resp.traj.r_gripper_poses.poses]
# get the manually measured transformation between the old and new target objects (just a translation for this test)
params = get_test_params()
actual_target_translation = jut.translation_matrix(params["translation"])
# find the expected warped gripper trajectory using the manual translation measurement
demo_spec_pt_traj_mats = get_demo_spec_pt_traj_mats(demo_tool_info, demo_target_data, gripper_data_key)
expected_spec_pt_traj = [np.dot(actual_target_translation, traj_mat) for traj_mat in demo_spec_pt_traj_mats]
expected_gripper_traj = get_expected_gripper_traj(exp_tool_info.special_point, expected_spec_pt_traj)
result = similar_trajectories(expected_gripper_traj, warped_grip_traj_mats)
report(result)
def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None):
markers = []
U = get_pr2_urdf()
link = U.links[linkname]
if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh):
rospy.logdebug("%s is a mesh. drawing it.", linkname)
marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD)
marker.ns = ns
marker.header = ps.header
linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation))
origFromLink = conversions.pose_to_hmat(ps.pose)
origFromGeom = np.dot(origFromLink, linkFromGeom)
marker.pose = conversions.hmat_to_pose(origFromGeom)
marker.scale = gm.Vector3(1,1,1)
marker.color = stdm.ColorRGBA(1,1,0,.5)
marker.id = 0
marker.lifetime = rospy.Duration()
marker.mesh_resource = str(link.visual.geometry.filename)
marker.mesh_use_embedded_materials = False
markers.append(marker)
else:
rospy.logdebug("%s is not a mesh", linkname)
if U.child_map.has_key(linkname):
for (cjoint,clink) in U.child_map[linkname]:
markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict))
return markers
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None):
rospy.logdebug("joint: %s"%jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0,0,0]
if not joint.origin.rotation: joint.origin.rotation = [0,0,0]
parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
def do_traj_ik_graph_search(pr2, lr, gripper_poses):
manip = get_manipulator(pr2, lr)
hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses]
def ikfunc(hmat):
return traj_ik_graph_search.ik_for_link(hmat, manip, "%s_gripper_tool_frame"%lr, return_all_solns=True)
pr2.update_rave()
start_joints = pr2.robot.GetDOFValues(manip.GetArmJoints())
robot = manip.GetRobot()
env = robot.GetEnv()
report = rave.CollisionReport()
link_info = []
def nodecost(joints):
robot.SetDOFValues(joints, manip.GetArmJoints())
return 100*env.CheckCollision(robot)
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost)
i_best = np.argmin(costs)
print "lowest cost of initial trajs:", costs[i_best]
best_path = paths[i_best]
return best_path
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None):
rospy.logdebug("joint: %s" % jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0, 0, 0]
if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0]
parentFromChild = conversions.trans_rot_to_hmat(
joint.origin.position,
transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(
valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(
np.array(joint.axis) * valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,
joint.child,
ns=ns,
valuedict=valuedict)
def test_cup_pour(demo_name, exp_name):
test_cup_pour_init()
verb_data_accessor = multi_item_verbs.VerbDataAccessor(test=True)
current_stage = 1
gripper_data_key = "r_gripper_tool_frame"
# info and data for previous stage
prev_demo_info = verb_data_accessor.get_stage_info(demo_name, current_stage-1)
prev_demo_data = verb_data_accessor.get_demo_data(prev_demo_info.stage_name)
prev_exp_info = verb_data_accessor.get_stage_info(exp_name, current_stage-1)
prev_exp_data = verb_data_accessor.get_demo_data(prev_exp_info.stage_name)
# info and data for current stage
cur_demo_info = verb_data_accessor.get_stage_info(demo_name, current_stage)
cur_demo_data = verb_data_accessor.get_demo_data(cur_demo_info.stage_name)
cur_exp_info = verb_data_accessor.get_stage_info(exp_name, current_stage)
cur_exp_data = verb_data_accessor.get_demo_data(cur_exp_info.stage_name)
# point clouds of tool for demo and experiment
prev_exp_pc = prev_exp_data["object_clouds"][prev_exp_info.item]["xyz"]
cur_exp_pc = cur_exp_data["object_clouds"][cur_exp_info.item]["xyz"]
# calculate the transformation from the world frame to the gripper frame
prev_exp_gripper_pos = prev_exp_data[gripper_data_key]["position"][-1]
prev_exp_gripper_orien = prev_exp_data[gripper_data_key]["orientation"][-1]
prev_world_to_gripper_trans = np.linalg.inv(juc.trans_rot_to_hmat(prev_exp_gripper_pos, prev_exp_gripper_orien))
gripper_frame_trans = make_verb_traj.make_to_gripper_frame_hmat(prev_world_to_gripper_trans)
warped_traj_resp = make_verb_traj.make_traj_multi_stage_do_work(cur_demo_info, [cur_exp_pc], None, current_stage, prev_demo_info, [prev_exp_pc], verb_data_accessor, gripper_frame_trans)
# get the actual transformation between the old and new target objects (just a translation for this test)
params = get_test_params()
translation = params['translation']
actual_target_translation_matrix = jut.translation_matrix(translation)
# get the demo special point trajectory
cur_demo_gripper_traj_xyzs = cur_demo_data[gripper_data_key]["position"]
cur_demo_gripper_traj_oriens = cur_demo_data[gripper_data_key]["orientation"]
cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)]
prev_demo_spec_pt_translation = jut.translation_matrix(np.array(prev_demo_info.special_point))
cur_demo_spec_pt_traj_as_mats = [np.dot(traj_mat, prev_demo_spec_pt_translation) for traj_mat in cur_demo_gripper_traj_mats]
# get the expected experiment special point trajectory
expected_spec_pt_traj = [np.dot(actual_target_translation_matrix, traj_mat) for traj_mat in cur_demo_spec_pt_traj_as_mats]
# get the expected experiment gripper trajectory
prev_exp_spec_pt_translation = jut.translation_matrix(np.array(prev_exp_info.special_point))
inv_cur_exp_spec_pt_translation = np.linalg.inv(prev_exp_spec_pt_translation)
expected_gripper_traj = [np.dot(traj_mat, inv_cur_exp_spec_pt_translation) for traj_mat in expected_spec_pt_traj]
# compare the expected new special point trajectory to the result of make_traj_multi_stage
result_traj = warped_traj_resp.traj
cur_exp_traj_as_mats = [juc.pose_to_hmat(pose) for pose in result_traj.r_gripper_poses.poses]
report(similar_trajectories(expected_gripper_traj, cur_exp_traj_as_mats))
def prepend_path_to_start(current_pos, gripper_poses, gripper_angles, n_steps):
before_traj = get_lin_interp_poses(current_pos,
juc.pose_to_hmat(gripper_poses[0]),
n_steps)
prepended_gripper_poses = np.concatenate(
(np.array(before_traj), gripper_poses))
updated_angles = np.concatenate(
(np.ones(n_steps) * gripper_angles[0], gripper_angles))
return prepended_gripper_poses, updated_angles
def do_traj_ik_graph_search_opt(pr2, lr, gripper_poses):
gripper_poses = [pose for i, pose in enumerate(gripper_poses) if i % 20 == 0]
graph_search_res = do_traj_ik_graph_search(pr2, lr, gripper_poses)
hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses]
poses = rave.poseFromMatrices(hmats)
quats = poses[:,:4]
xyzs = poses[:,4:7]
manip_name = {"r":"rightarm", "l":"leftarm"}[lr]
request = get_ik_request(manip_name, "%s_gripper_tool_frame"%lr,
xyzs[-1], quats[-1], graph_search_res, xyzs, quats)
s = json.dumps(request)
print "REQUEST:", s
pr2.robot.SetDOFValues(graph_search_res[0], pr2.robot.GetManipulator(manip_name).GetArmIndices())
trajoptpy.SetInteractive(True);
prob = trajoptpy.ConstructProblem(s, pr2.env)
result = trajoptpy.OptimizeProblem(prob)
def do_traj_ik_graph_search(lr, gripper_poses):
hmats = [conversions.pose_to_hmat(pose) for pose in gripper_poses]
def ikfunc(hmat):
return traj_ik_graph_search.ik_for_link(hmat, manip, "%s_gripper_tool_frame"%lr, return_all_solns=True)
manip = get_manipulator(lr)
start_joints = Globals.pr2.robot.GetDOFValues(manip.GetArmJoints())
def nodecost(joints):
robot = manip.GetRobot()
robot.SetDOFValues(joints, manip.GetArmJoints())
return 100*robot.GetEnv().CheckCollision(robot)
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost)
i_best = np.argmin(costs)
print "lowest cost of initial trajs:", costs[i_best]
path_init = paths[i_best]
return path_init
def make_kin_tree_from_link(ps, linkname, ns='default_ns', valuedict=None):
markers = []
U = get_pr2_urdf()
link = U.links[linkname]
if link.visual and link.visual.geometry and isinstance(
link.visual.geometry, urdf.Mesh):
rospy.logdebug("%s is a mesh. drawing it.", linkname)
marker = Marker(type=Marker.MESH_RESOURCE, action=Marker.ADD)
marker.ns = ns
marker.header = ps.header
linkFromGeom = conversions.trans_rot_to_hmat(
link.visual.origin.position,
transformations.quaternion_from_euler(
*link.visual.origin.rotation))
origFromLink = conversions.pose_to_hmat(ps.pose)
origFromGeom = np.dot(origFromLink, linkFromGeom)
marker.pose = conversions.hmat_to_pose(origFromGeom)
marker.scale = gm.Vector3(1, 1, 1)
marker.color = stdm.ColorRGBA(1, 1, 0, .5)
marker.id = 0
marker.lifetime = rospy.Duration()
marker.mesh_resource = str(link.visual.geometry.filename)
marker.mesh_use_embedded_materials = False
markers.append(marker)
else:
rospy.logdebug("%s is not a mesh", linkname)
if U.child_map.has_key(linkname):
for (cjoint, clink) in U.child_map[linkname]:
markers.extend(
make_kin_tree_from_joint(ps,
cjoint,
ns=ns,
valuedict=valuedict))
return markers
def test_translation(demo_name, exp_name, data_dir):
translation_test_init()
verb_data_accessor = multi_item_verbs.VerbDataAccessor(
test_info_dir=osp.join("test", TEST_DATA_DIR, data_dir))
current_stage = 1
# info and data for tool stage
demo_tool_info = verb_data_accessor.get_stage_info(demo_name,
current_stage - 1)
demo_tool_data = verb_data_accessor.get_demo_stage_data(
demo_tool_info.stage_name)
exp_tool_info = verb_data_accessor.get_stage_info(exp_name,
current_stage - 1)
exp_tool_data = verb_data_accessor.get_demo_stage_data(
exp_tool_info.stage_name)
# info and data for target stage
demo_target_info = verb_data_accessor.get_stage_info(
demo_name, current_stage)
demo_target_data = verb_data_accessor.get_demo_stage_data(
demo_target_info.stage_name)
exp_target_info = verb_data_accessor.get_stage_info(
exp_name, current_stage)
exp_target_data = verb_data_accessor.get_demo_stage_data(
exp_target_info.stage_name)
gripper_data_key = "%s_gripper_tool_frame" % demo_target_info.arms_used
# point clouds of tool for demo and experiment
exp_tool_pc = exp_tool_data["object_cloud"][exp_tool_info.item]["xyz"]
exp_target_pc = exp_target_data["object_cloud"][
exp_target_info.item]["xyz"]
# calculate the transformation from the world frame to the gripper frame in the experiment scene
world_to_grip_transform = get_world_to_grip_exp_transform(
exp_tool_data, gripper_data_key)
world_to_grip_transform_func = multi_item_make_verb_traj.make_world_to_grip_transform_hmat(
world_to_grip_transform)
warped_traj_resp = multi_item_make_verb_traj.make_traj_multi_stage_do_work(
demo_name, exp_target_pc, None, current_stage, demo_tool_info,
exp_tool_pc, verb_data_accessor, world_to_grip_transform_func, "tps")
# assuming that the arms_used for the target stage is 'l' or 'r'
if demo_target_info.arms_used == 'l':
warped_grip_traj_mats = [
juc.pose_to_hmat(pose)
for pose in warped_traj_resp.traj.l_gripper_poses.poses
]
elif demo_target_info.arms_used == 'r':
warped_grip_traj_mats = [
juc.pose_to_hmat(pose)
for pose in warped_traj_resp.traj.r_gripper_poses.poses
]
# get the manually measured transformation between the old and new target objects (just a translation for this test)
params = get_test_params()
actual_target_translation = jut.translation_matrix(params["translation"])
# find the expected warped gripper trajectory using the manual translation measurement
demo_spec_pt_traj_mats = get_demo_spec_pt_traj_mats(
demo_target_data, gripper_data_key)
expected_spec_pt_traj = [
np.dot(actual_target_translation, traj_mat)
for traj_mat in demo_spec_pt_traj_mats
]
expected_gripper_traj = get_expected_gripper_traj(
exp_tool_info.special_point, expected_spec_pt_traj)
result = similar_trajectories(expected_gripper_traj, warped_grip_traj_mats)
report(result)
def prepend_path_to_start(current_pos, gripper_poses, gripper_angles, n_steps):
before_traj = get_lin_interp_poses(current_pos, juc.pose_to_hmat(gripper_poses[0]), n_steps)
prepended_gripper_poses = np.concatenate((np.array(before_traj), gripper_poses))
updated_angles = np.concatenate((np.ones(n_steps) * gripper_angles[0], gripper_angles))
return prepended_gripper_poses, updated_angles
