def make_traj(req):
"""
Generate a trajectory using warping
See MakeTrajectory service description
(TODO) should be able to specify a specific demo
"""
assert isinstance(req, MakeTrajectoryRequest)
new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds]
scene_info = "PLACEHOLDER"
best_demo_name, best_demo_info = verbs.get_closest_demo(
req.verb, scene_info)
best_demo_data = verbs.get_demo_data(best_demo_name)
transform_type = "tps"
old_object_clouds = [
best_demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in best_demo_data["object_clouds"].keys()
]
if len(old_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(old_object_clouds[0], .02)
y_md = voxel_downsample(new_object_clouds[0], .02)
if transform_type == "tps":
#warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
warp = registration.tps_rpm(x_nd,
y_md,
plotting=2,
reg_init=2,
reg_final=.05,
n_iter=10,
verbose=False)
elif transform_type == "translation2d":
warp = registration.Translation2d()
warp.fit(x_nd, y_md)
elif transform_type == "rigid2d":
warp = registration.Rigid2d()
warp.fit(x_nd, y_md)
else:
raise Exception("transform type %s is not yet implemented" %
transform_type)
l_offset, r_offset = np.zeros(3), np.zeros(3)
#if "r_tool" in best_demo_info:
#r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"])
#if "l_tool" in best_demo_info:
#l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"])
arms_used = best_demo_info["arms_used"]
warped_demo_data = warping.transform_verb_demo(warp, best_demo_data)
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
if arms_used in "lb":
traj.l_gripper_poses.poses = juc.xyzs_quats_to_poses(
warped_demo_data["l_gripper_tool_frame"]["position"],
warped_demo_data["l_gripper_tool_frame"]["orientation"])
traj.l_gripper_angles = warped_demo_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = req.object_clouds[
0].header.frame_id
if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0
if arms_used in "rb":
traj.r_gripper_poses.poses = juc.xyzs_quats_to_poses(
warped_demo_data["r_gripper_tool_frame"]["position"],
warped_demo_data["r_gripper_tool_frame"]["orientation"])
traj.r_gripper_angles = warped_demo_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = req.object_clouds[
0].header.frame_id
if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0
Globals.handles = []
plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj)
pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
Globals.handles.append(
Globals.rviz.draw_curve(pose_array,
rgba=(0, 0, 1, 1),
width=.01,
type=Marker.CUBE_LIST))
return resp
from lfd import verbs, make_verb_traj, exec_verb_traj
from verb_msgs.srv import *
import rospy
import numpy as np
import brett2.ros_utils as ru
import roslib; roslib.load_manifest("snazzy_msgs")
from snazzy_msgs.srv import *
import sensor_msgs.msg as sm
from jds_utils.yes_or_no import yes_or_no
assert args.demo is not None or args.verb is not None
if args.demo is not None:
demo_name = args.demo
print "using demo", args.demo
else:
demo_name, _ = verbs.get_closest_demo(args.verb, "sdkfjsldk")
if rospy.get_name() == "/unnamed":
rospy.init_node("test_get_verb_traj_service",disable_signals=True)
make_verb_traj.Globals.setup()
exec_verb_traj.Globals.setup()
pr2 = exec_verb_traj.Globals.pr2
pr2.rgrip.open()
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
pr2.join_all()
demo_info = verbs.get_demo_info(demo_name)
demo_data = verbs.get_demo_data(demo_name)
from verb_msgs.srv import *
import rospy
import numpy as np
import brett2.ros_utils as ru
import roslib
roslib.load_manifest("snazzy_msgs")
from snazzy_msgs.srv import *
import sensor_msgs.msg as sm
from jds_utils.yes_or_no import yes_or_no
assert args.demo is not None or args.verb is not None
if args.demo is not None:
demo_name = args.demo
print "using demo", args.demo
else:
demo_name, _ = verbs.get_closest_demo(args.verb, "sdkfjsldk")
if rospy.get_name() == "/unnamed":
rospy.init_node("test_get_verb_traj_service", disable_signals=True)
make_verb_traj.Globals.setup()
exec_verb_traj.Globals.setup()
pr2 = exec_verb_traj.Globals.pr2
pr2.rgrip.open()
pr2.lgrip.open()
pr2.rarm.goto_posture('side')
pr2.larm.goto_posture('side')
pr2.join_all()
demo_info = verbs.get_demo_info(demo_name)
demo_data = verbs.get_demo_data(demo_name)
def make_traj(req):
"""
Generate a trajectory using warping
See MakeTrajectory service description
(TODO) should be able to specify a specific demo
"""
assert isinstance(req, MakeTrajectoryRequest)
new_object_clouds = [pc2xyzrgb(cloud)[0] for cloud in req.object_clouds]
scene_info = "PLACEHOLDER"
best_demo_name, best_demo_info = verbs.get_closest_demo(req.verb, scene_info)
best_demo_data = verbs.get_demo_data(best_demo_name)
transform_type = "tps"
old_object_clouds = [best_demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in best_demo_data["object_clouds"].keys()]
if len(old_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(old_object_clouds[0],.02)
y_md = voxel_downsample(new_object_clouds[0],.02)
if transform_type == "tps":
#warp = registration.tps_rpm_zrot(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
warp = registration.tps_rpm(x_nd, y_md, plotting=2,reg_init=2,reg_final=.05, n_iter=10, verbose=False)
elif transform_type == "translation2d":
warp = registration.Translation2d()
warp.fit(x_nd, y_md)
elif transform_type == "rigid2d":
warp = registration.Rigid2d()
warp.fit(x_nd, y_md)
else:
raise Exception("transform type %s is not yet implemented"%transform_type)
l_offset,r_offset = np.zeros(3), np.zeros(3)
#if "r_tool" in best_demo_info:
#r_offset = asarray(tool_info[this_verb_info["r_tool"]]["translation"])
#if "l_tool" in best_demo_info:
#l_offset = asarray(tool_info[this_verb_info["l_tool"]]["translation"])
arms_used = best_demo_info["arms_used"]
warped_demo_data = warping.transform_verb_demo(warp, best_demo_data)
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
if arms_used in "lb":
traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["l_gripper_tool_frame"]["position"], warped_demo_data["l_gripper_tool_frame"]["orientation"])
traj.l_gripper_angles = warped_demo_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id
if "l_tool" in best_demo_info: traj.l_gripper_angles *= 0
if arms_used in "rb":
traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_demo_data["r_gripper_tool_frame"]["position"], warped_demo_data["r_gripper_tool_frame"]["orientation"])
traj.r_gripper_angles = warped_demo_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = req.object_clouds[0].header.frame_id
if "r_tool" in best_demo_info: traj.r_gripper_angles *= 0
Globals.handles = []
plot_original_and_warped_demo(best_demo_data, warped_demo_data, traj)
pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST))
return resp
