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 get_demo_spec_pt_traj_mats(demo_tool_info, demo_target_data, gripper_data_key):
demo_grip_traj_xyzs = demo_target_data[gripper_data_key]["position"]
demo_grip_traj_oriens = demo_target_data[gripper_data_key]["orientation"]
demo_grip_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_grip_traj_xyzs, demo_grip_traj_oriens)]
demo_tool_spec_pt_translation = jut.translation_matrix(np.array(demo_tool_info.special_point))
demo_spec_pt_traj_mats = [np.dot(traj_mat, demo_tool_spec_pt_translation) for traj_mat in demo_grip_traj_mats]
return demo_spec_pt_traj_mats
def make_joint_traj(xyzs, quats, manip, ref_frame, targ_frame, filter_options = 0):
"do ik and then fill in the points where ik failed"
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
joint_inds = manip.GetArmIndices()
robot.SetActiveDOFs(joint_inds)
orig_joint = robot.GetActiveDOFValues()
joints = []
inds = []
for i in xrange(0,n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetActiveDOFValues(joint)
robot.SetActiveDOFValues(orig_joint)
rospy.loginfo("found ik soln for %i of %i points",len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
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 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 __init__(self):
PlannerPR2.__init__(self)
rospy.wait_for_service('getCutLine')
self.cutService = rospy.ServiceProxy('getCutLine',PointDir)
rospy.wait_for_service('getHoleNormal')
self.holeService = rospy.ServiceProxy('getHoleNormal', PointDir)
self.camera_frame = '/camera_rgb_optical_frame'
self.tf_listener.waitForTransform("/base_footprint", self.camera_frame, rospy.Time(), rospy.Duration(10.0))
(trans, rot) = self.tf_listener.lookupTransform('/base_footprint',self.camera_frame,rospy.Time(0))
# basefootprintFromCamera, assuming transform doesn't change. If it does, need to do this often.
self.camera_transform = conv.trans_rot_to_hmat(trans, rot);
# Initial index of cut picked up. Rest of procedure would be done taking this into account.
self.init_index = 0
# Variables for flap pick-up
# How far above the cut to start
self.cut_upStartPos = 0.05
# How far to the side to start (+ve direction is opposite to the side of the cut)
# i.e, for left cut, +ve is right and vice versa
self.cut_sideStartPos = -0.00
# Rotation of gripper downward
self.cut_rotStartPos = np.pi/4
# Initial gripper angle to hold cut
self.cut_gripperStartAngle = 0.04
# How much to move down to pick up flap
self.cut_moveDown = 0.1
# How much to move in to pick up flap
self.cut_moveIn = 0.05
# How much to move up after picking up the flap
self.cut_moveUpEnd = 0.045
# How much to move in after picking up flap
self.cut_moveInEnd = 0.015
# Distance to move towards hole from start position to pierce
self.hole_moveToward = 0.025
# Angle to move in circle to pierce cut
self.hole_finAng = np.pi/3
# Angle to reverse in circle after piercing cut
self.hole_returnAng = -np.pi/7
# The initial hole pierce point. This is assumed to be a point on the needle during the pierce.
self.init_holePt = np.array([0,0,0])
# The final hole exit point.
self.final_holePt = np.array([0,0,0])
# Angle to move in to pierce the second hole
# self.secondPierce_angle = np.pi/1.9
# If needle is not moving back after first pierce
self.secondPierce_angle = np.pi/2.5
# Distance behind the needle the regrasping starts.
self.regrasp_initDist = 0.05
# Max effort for closing the gripper
self.regrasp_closeMaxEffort = 80
# Angle to move through in order to pull out the needle
self.regrasp_removalAng = np.pi/1.4
def transform_points(xyz, to_frame, from_frame):
xyz = xyz.reshape(-1,3)
xyz1 = np.c_[xyz, np.ones((xyz.shape[0],1))]
trans, rot = brett.tf_listener.lookupTransform(to_frame, from_frame,rospy.Time(0))
mat = conv.trans_rot_to_hmat(trans,rot)
xyz1_transformed = np.dot(xyz1, mat.T)
return xyz1_transformed[:,:3]
def show_ik_solns(part_name, manip, i, warped_demo):
frame = "%s_gripper_tool_frame"%part_name[0]
hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i], warped_demo[frame]['orientation'][i])
solns = traj_ik_graph_search.ik_for_link(hmat, manip, frame, return_all_solns=True)
# newrobots = []
print 'Displaying', len(solns), 'IK solutions for', part_name, 'at time', i
print solns
def lin_rigid_tps_transform(tps_transform, lin_point):
lin_ag, trans_g, w_ng, x_na = tps_transform.lin_ag, tps_transform.trans_g, tps_transform.w_ng, tps_transform.x_na
# orientation part is gradient(f)
orien = tps.tps_grad(np.array([lin_point]), lin_ag, trans_g, w_ng, x_na)[0]
# translation part is f(a) - gradient(f)*a
trans = tps.tps_eval(np.array([lin_point]), lin_ag, trans_g, w_ng, x_na)[0] - np.dot(orien, lin_point)
linearized_transform = juc.trans_rot_to_hmat(trans, juc.mat2quat(orthogonalize3_cross(np.array([orien]))))
return linearized_transform
def update_rave(self, use_map = False):
ros_values = self.get_last_joint_message().position
rave_values = [ros_values[i_ros] for i_ros in self.good_ros_inds]
self.robot.SetJointValues(rave_values[:20],self.rave_inds[:20])
self.robot.SetJointValues(rave_values[20:],self.rave_inds[20:])
if use_map:
(trans,rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))
self.robot.SetTransform(conv.trans_rot_to_hmat(trans, rot))
def transform_points(xyz, to_frame, from_frame):
xyz = xyz.reshape(-1, 3)
xyz1 = np.c_[xyz, np.ones((xyz.shape[0], 1))]
trans, rot = brett.tf_listener.lookupTransform(to_frame, from_frame,
rospy.Time(0))
mat = conv.trans_rot_to_hmat(trans, rot)
xyz1_transformed = np.dot(xyz1, mat.T)
return xyz1_transformed[:, :3]
def get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform):
demo_spec_pt_traj_xyzs, demo_spec_pt_traj_oriens = [], []
for demo_spec_pt_traj_mat in demo_spec_pt_traj_mats:
demo_spec_pt_traj_xyz, demo_spec_pt_traj_orien = juc.hmat_to_trans_rot(demo_spec_pt_traj_mat)
demo_spec_pt_traj_xyzs.append(demo_spec_pt_traj_xyz)
demo_spec_pt_traj_oriens.append(juc.quat2mat(demo_spec_pt_traj_orien))
warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens = demo_to_exp_target_transform.transform_frames(np.array(demo_spec_pt_traj_xyzs), np.array(demo_spec_pt_traj_oriens))
warped_spec_pt_traj_mats = [juc.trans_rot_to_hmat(warped_spec_pt_traj_xyz, juc.mat2quat(warped_spec_pt_traj_orien)) for warped_spec_pt_traj_xyz, warped_spec_pt_traj_orien in zip(warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens)]
return warped_spec_pt_traj_mats
def get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform):
demo_spec_pt_traj_xyzs, demo_spec_pt_traj_oriens = [], []
for demo_spec_pt_traj_mat in demo_spec_pt_traj_mats:
demo_spec_pt_traj_xyz, demo_spec_pt_traj_orien = juc.hmat_to_trans_rot(demo_spec_pt_traj_mat)
demo_spec_pt_traj_xyzs.append(demo_spec_pt_traj_xyz)
demo_spec_pt_traj_oriens.append(juc.quat2mat(demo_spec_pt_traj_orien))
warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens = demo_to_exp_target_transform.transform_frames(np.array(demo_spec_pt_traj_xyzs), np.array(demo_spec_pt_traj_oriens))
warped_spec_pt_traj_mats = [juc.trans_rot_to_hmat(warped_spec_pt_traj_xyz, juc.mat2quat(warped_spec_pt_traj_orien)) for warped_spec_pt_traj_xyz, warped_spec_pt_traj_orien in zip(warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens)]
return warped_spec_pt_traj_mats
def show_ik_solns(part_name, manip, i, warped_demo):
frame = "%s_gripper_tool_frame" % part_name[0]
hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i],
warped_demo[frame]['orientation'][i])
solns = traj_ik_graph_search.ik_for_link(hmat,
manip,
frame,
return_all_solns=True)
# newrobots = []
print 'Displaying', len(solns), 'IK solutions for', part_name, 'at time', i
print solns
def compute_feas_inds(xyzs, quats, manip, targ_frame, check_collisions=False):
assert(len(xyzs) == len(quats))
hmats = [conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats)]
link = manip.GetRobot().GetLink(targ_frame)
Tcur_w_link = link.GetTransform()
Tcur_w_ee = manip.GetEndEffectorTransform()
Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee)
def ikfunc(hmat):
return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16 + (1 if check_collisions else 0))
return traj_ik_graph_search.compute_feas_inds(hmats, ikfunc)
def make_joint_traj_by_graph_search(xyzs,
quats,
manip,
targ_frame,
downsample=1,
check_collisions=False):
assert (len(xyzs) == len(quats))
hmats = [
conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats)
]
ds_hmats = hmats[::downsample]
orig_len, ds_len = len(hmats), len(ds_hmats)
if downsample != 1:
rospy.loginfo(
'Note: downsampled %s trajectory from %d points to %d points',
manip.GetName(), orig_len, ds_len)
link = manip.GetRobot().GetLink(targ_frame)
Tcur_w_link = link.GetTransform()
Tcur_w_ee = manip.GetEndEffectorTransform()
Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee)
def ikfunc(hmat):
return manip.FindIKSolutions(hmat.dot(Tf_link_ee),
2 + 16 + (1 if check_collisions else 0))
def nodecost(joints):
robot = manip.GetRobot()
cost = 0
with robot:
robot.SetDOFValues(joints, manip.GetArmIndices())
cost = 100 * robot.GetEnv().CheckCollision(robot)
return cost
start_joints = manip.GetRobot().GetDOFValues(manip.GetArmIndices())
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(
ds_hmats,
ikfunc,
start_joints=start_joints,
nodecost=nodecost if check_collisions else None)
rospy.loginfo("%s: %i of %i points feasible", manip.GetName(),
len(timesteps), ds_len)
i_best = np.argmin(costs)
rospy.loginfo("lowest cost of initial trajs: %f", costs[i_best])
path_init = unwrap_arm_traj(paths[i_best])
path_init_us = mu.interp2d(range(orig_len),
range(orig_len)[::downsample],
path_init) # un-downsample
assert len(path_init_us) == orig_len
return path_init_us, timesteps
def get_demo_spec_pt_traj_mats(demo_target_data, gripper_data_key):
demo_grip_traj_xyzs = demo_target_data[gripper_data_key]["position"]
demo_grip_traj_oriens = demo_target_data[gripper_data_key]["orientation"]
demo_grip_traj_mats = [
juc.trans_rot_to_hmat(trans, orien)
for (trans, orien) in zip(demo_grip_traj_xyzs, demo_grip_traj_oriens)
]
demo_tool_spec_pt_translation = jut.translation_matrix(
np.array(demo_tool_info.special_point))
demo_spec_pt_traj_mats = [
np.dot(traj_mat, demo_tool_spec_pt_translation)
for traj_mat in demo_grip_traj_mats
]
return demo_spec_pt_traj_mats
def get_warped_grip_traj_mats(warped_spec_pt_traj_mats, tool_stage_data, demo_to_exp_tool_transform, spec_pt_in_grip, world_to_grip_transform_func, arm):
if demo_to_exp_tool_transform is None:
grip_to_spec_pt_transform = np.linalg.inv(jut.translation_matrix(spec_pt_in_grip))
else:
demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm)
demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip)
demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world)
# find the transformation from the new special point to the gripper frame
warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame)
warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame)
warped_spec_pt_in_grip_trans = world_to_grip_transform_func(warped_spec_pt_in_world_trans)
warped_spec_pt_in_grip_rot = warped_spec_pt_in_world_rot
warped_spec_pt_in_grip_frame = juc.trans_rot_to_hmat(warped_spec_pt_in_grip_trans, warped_spec_pt_in_grip_rot)
print_hmat_info(warped_spec_pt_in_grip_frame, "warped_spec_pt_in_grip_frame")
# transform the warped special point trajectory back to a gripper trajectory in the experiment
grip_to_spec_pt_transform = np.linalg.inv(warped_spec_pt_in_grip_frame)
warped_grip_traj_mats = [np.dot(spec_pt_mat, grip_to_spec_pt_transform) for spec_pt_mat in warped_spec_pt_traj_mats]
return warped_grip_traj_mats
def get_warped_grip_traj_mats(warped_spec_pt_traj_mats, tool_stage_data, demo_to_exp_tool_transform, spec_pt_in_grip, world_to_grip_transform_func, arm):
if demo_to_exp_tool_transform is None:
grip_to_spec_pt_transform = np.linalg.inv(jut.translation_matrix(spec_pt_in_grip))
else:
demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm)
demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip)
demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world)
# find the transformation from the new special point to the gripper frame
warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame)
warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame)
warped_spec_pt_in_grip_trans = world_to_grip_transform_func(warped_spec_pt_in_world_trans)
warped_spec_pt_in_grip_rot = warped_spec_pt_in_world_rot
warped_spec_pt_in_grip_frame = juc.trans_rot_to_hmat(warped_spec_pt_in_grip_trans, warped_spec_pt_in_grip_rot)
print_hmat_info(warped_spec_pt_in_grip_frame, "warped_spec_pt_in_grip_frame")
# transform the warped special point trajectory back to a gripper trajectory in the experiment
grip_to_spec_pt_transform = np.linalg.inv(warped_spec_pt_in_grip_frame)
warped_grip_traj_mats = [np.dot(spec_pt_mat, grip_to_spec_pt_transform) for spec_pt_mat in warped_spec_pt_traj_mats]
return warped_grip_traj_mats
def transform_points(xyz, listener, to_frame, from_frame,n_tries=10):
#listener.waitForTransform(to_frame, from_frame, rospy.Time.now(), rospy.Duration(1))
for i in xrange(n_tries):
try:
trans, rot = listener.lookupTransform(to_frame, from_frame, rospy.Time(0))
break
except Exception:
print "tf exception:"
print colorize.colorize(traceback.format_exc(),"yellow")
rospy.sleep(.1)
time.sleep(.05)
if i == n_tries-1: raise Exception("f**k tf")
hmat = conversions.trans_rot_to_hmat(trans,rot)
xyz1 = np.c_[xyz.reshape(-1,3), np.ones((xyz.size/3,1))]
xyz1_tf = np.dot(xyz1, hmat.T)
xyz_tf = xyz1_tf[:,:3].reshape(xyz.shape)
return xyz_tf
def transform_points(xyz, listener, to_frame, from_frame, n_tries=10):
#listener.waitForTransform(to_frame, from_frame, rospy.Time.now(), rospy.Duration(1))
for i in xrange(n_tries):
try:
trans, rot = listener.lookupTransform(to_frame, from_frame,
rospy.Time(0))
break
except Exception:
print "tf exception:"
print colorize.colorize(traceback.format_exc(), "yellow")
rospy.sleep(.1)
time.sleep(.05)
if i == n_tries - 1: raise Exception("f**k tf")
hmat = conversions.trans_rot_to_hmat(trans, rot)
xyz1 = np.c_[xyz.reshape(-1, 3), np.ones((xyz.size / 3, 1))]
xyz1_tf = np.dot(xyz1, hmat.T)
xyz_tf = xyz1_tf[:, :3].reshape(xyz.shape)
return xyz_tf
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_joint_traj_by_graph_search(xyzs, quats, manip, targ_frame, downsample=1, check_collisions=False):
assert(len(xyzs) == len(quats))
hmats = [conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats)]
ds_hmats = hmats[::downsample]
orig_len, ds_len = len(hmats), len(ds_hmats)
if downsample != 1:
rospy.loginfo('Note: downsampled %s trajectory from %d points to %d points', manip.GetName(), orig_len, ds_len)
link = manip.GetRobot().GetLink(targ_frame)
Tcur_w_link = link.GetTransform()
Tcur_w_ee = manip.GetEndEffectorTransform()
Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee)
def ikfunc(hmat):
return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16 + (1 if check_collisions else 0))
#return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16)
def nodecost(joints):
robot = manip.GetRobot()
cost = 0
with robot:
robot.SetDOFValues(joints, manip.GetArmIndices())
cost = 100*robot.GetEnv().CheckCollision(robot)
return cost
start_joints = manip.GetRobot().GetDOFValues(manip.GetArmIndices())
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(
ds_hmats, ikfunc,
start_joints=start_joints
#nodecost=nodecost if check_collisions else None
)
rospy.loginfo("%s: %i of %i points feasible", manip.GetName(), len(timesteps), ds_len)
i_best = np.argmin(costs)
rospy.loginfo("lowest cost of initial trajs: %f", costs[i_best])
path_init = unwrap_arm_traj(paths[i_best])
path_init_us = mu.interp2d(range(orig_len), range(orig_len)[::downsample], path_init) # un-downsample
assert len(path_init_us) == orig_len
return path_init_us, timesteps
def make_joint_traj(xyzs, quats, joint_seeds, manip, ref_frame, targ_frame,
filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0, n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame,
filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
rospy.loginfo("found ik soln for %i of %i points", len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
def make_joint_traj(xyzs, quats, joint_seeds,manip, ref_frame, targ_frame,filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0,n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
rospy.loginfo("found ik soln for %i of %i points",len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
def make_joint_traj(xyzs,
quats,
manip,
ref_frame,
targ_frame,
filter_options=0):
"do ik and then fill in the points where ik failed"
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
joint_inds = manip.GetArmIndices()
robot.SetActiveDOFs(joint_inds)
orig_joint = robot.GetActiveDOFValues()
joints = []
inds = []
for i in xrange(0, n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame,
filter_options)
if joint is not None:
joints.append(joint)
inds.append(i)
robot.SetActiveDOFValues(joint)
robot.SetActiveDOFValues(orig_joint)
rospy.loginfo("found ik soln for %i of %i points", len(inds), n)
if len(inds) > 2:
joints2 = mu.interp2d(np.arange(n), inds, joints)
return joints2, inds
else:
return np.zeros((n, len(joints))), []
def ros_transform_to_hmat(tros):
return conversions.trans_rot_to_hmat(
[tros.translation.x, tros.translation.y, tros.translation.z],
[tros.rotation.x, tros.rotation.y, tros.rotation.z, tros.rotation.w])
robot = env.GetRobots()[0]
manip = robot.SetActiveManipulator('leftarm')
lower, upper = robot.GetDOFLimits(
manip.GetArmIndices()) # get the limits of just the arm indices
ikmodel = databases.inversekinematics.InverseKinematicsModel(
robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
n_iter = 1000
rand_joints = []
for i in range(n_iter):
rand_joints.append(lower + numpy.random.rand(len(lower)) * (upper - lower))
rand_hmats = [
conv.trans_rot_to_hmat(
*traj_opt.joints2xyzquat(manip, 'l_gripper_tool_frame', j))
for j in rand_joints
]
start_time = time.time()
solns = []
for hmat in Bar().iter(rand_hmats):
s = numpy.array(
traj_ik_graph_search.ik_for_link(hmat,
manip,
'l_gripper_tool_frame',
return_all_solns=True))
if s.size == 0: continue
solns.append(s)
end_time = time.time()
print 'ik time: ', end_time - start_time, ' | ', (end_time -
def get_world_to_grip_exp_transform(exp_tool_data, gripper_data_key):
exp_gripper_pos = exp_tool_data[gripper_data_key]["position"][-1]
exp_gripper_orien = exp_tool_data[gripper_data_key]["orientation"][-1]
world_to_grip_transform = np.linalg.inv(juc.trans_rot_to_hmat(exp_gripper_pos, exp_gripper_orien))
return world_to_grip_transform
def apply_tps_transform_to_hmat(tps_transform, hmat):
xyz, quat = juc.hmat_to_trans_rot(hmat)
warped_xyz, warped_mat = tps_transform.transform_frames(np.array([xyz]), np.array([juc.quat2mat(quat)]))
return juc.trans_rot_to_hmat(warped_xyz[0], juc.mat2quat(warped_mat[0]))
import roslib
roslib.load_manifest('tf')
import tf
import rospy
import sensor_msgs.msg as sm
import numpy as np
from jds_utils import conversions
rospy.init_node("save_point_cloud_data")
listener = tf.TransformListener()
rospy.sleep(.5)
xyzs = []
while True:
response = raw_input(
"press enter when you're ready. type 'quit' when done")
if 'quit' in response: break
pc = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2)
trans, rot = listener.lookupTransform("base_footprint", pc.header.frame_id,
rospy.Time(0))
hmat = conversions.trans_rot_to_hmat(trans, rot)
xyz, rgb = ros_utils.pc2xyzrgb(pc)
xyz = np.squeeze(xyz)
xyz1 = np.c_[xyz, np.ones((len(xyz), 1))]
xyz1_base = np.dot(xyz1, hmat.T)
xyzs.append(xyz1_base[:, :3])
print "saving"
np.save("/home/joschu/Data/rope/ropes.npy", np.array(xyzs, dtype=object))
def get_world_to_grip_exp_transform(exp_tool_data, gripper_data_key):
exp_gripper_pos = exp_tool_data[gripper_data_key]["position"][-1]
exp_gripper_orien = exp_tool_data[gripper_data_key]["orientation"][-1]
world_to_grip_transform = np.linalg.inv(
juc.trans_rot_to_hmat(exp_gripper_pos, exp_gripper_orien))
return world_to_grip_transform
def ros_transform_to_hmat(tros):
return conversions.trans_rot_to_hmat(
[tros.translation.x, tros.translation.y, tros.translation.z],
[tros.rotation.x, tros.rotation.y, tros.rotation.z, tros.rotation.w],
)
env=Environment()
env.Load('robots/pr2-beta-static.zae')
robot=env.GetRobots()[0]
manip = robot.SetActiveManipulator('leftarm')
lower,upper = robot.GetDOFLimits(manip.GetArmIndices()) # get the limits of just the arm indices
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
n_iter = 1000
rand_joints = []
for i in range(n_iter):
rand_joints.append(lower+numpy.random.rand(len(lower))*(upper-lower))
rand_hmats = [conv.trans_rot_to_hmat(*traj_opt.joints2xyzquat(manip, 'l_gripper_tool_frame', j)) for j in rand_joints]
start_time = time.time()
solns = []
for hmat in Bar().iter(rand_hmats):
s = numpy.array(traj_ik_graph_search.ik_for_link(hmat, manip, 'l_gripper_tool_frame', return_all_solns=True))
if s.size == 0: continue
solns.append(s)
end_time = time.time()
print 'ik time: ', end_time-start_time, ' | ', (end_time-start_time)/n_iter, 'each'
start_time = time.time()
for i in Bar().iter(range(len(solns)-1)):
a, b, c = traj_ik_graph_search.build_graph_part(None, solns[i], solns[i+1])
end_time = time.time()
import kinematics.reachability as kr
import openravepy
from jds_utils import conversions
import numpy as np
poses = []
poses.append(((0,.6,1), (0,0,0,1)))
poses.append(((0,.7,1), (0,0,0,1)))
poses.append(((0,.8,1), (0,0,0,1)))
env = openravepy.Environment()
env.Load("./tablescene.xml")
assert env is not None
robot = env.GetRobots()[0]
leftarm = robot.GetManipulator("leftarm")
db = kr.ReachabilityDatabase("read")
(x, y), cost = kr.find_min_cost_base(db, robot, "leftarm", poses, plotting=True)
print cost
base_pose = np.eye(4)
base_pose[0,3] = x
base_pose[1,3] = y
robot.SetTransform(base_pose)
mats = [conversions.trans_rot_to_hmat(trans,rot) for (trans,rot) in poses]
for mat in mats:
solns = leftarm.FindIKSolutions(mat,2+16)
print "num solutions",len(solns)
def get_demo_grip_traj_mats(target_stage_data, arm):
gripper_data_key = "%s_gripper_tool_frame" % (arm)
demo_target_grip_traj_xyzs = target_stage_data[gripper_data_key]["position"]
demo_target_grip_traj_oriens = target_stage_data[gripper_data_key]["orientation"]
demo_target_grip_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_target_grip_traj_xyzs, demo_target_grip_traj_oriens)]
return demo_target_grip_traj_mats
def apply_tps_transform_to_hmat(tps_transform, hmat):
xyz, quat = juc.hmat_to_trans_rot(hmat)
warped_xyz, warped_mat = tps_transform.transform_frames(np.array([xyz]), np.array([juc.quat2mat(quat)]))
return juc.trans_rot_to_hmat(warped_xyz[0], juc.mat2quat(warped_mat[0]))
def get_demo_tool_grip_to_world_transform(tool_stage_data, arm):
gripper_data_key = "%s_gripper_tool_frame" % (arm)
demo_tool_grip_pos = tool_stage_data[gripper_data_key]["position"][-1]
demo_tool_grip_orien = tool_stage_data[gripper_data_key]["orientation"][-1]
demo_tool_grip_to_world_transform = juc.trans_rot_to_hmat(demo_tool_grip_pos, demo_tool_grip_orien)
return demo_tool_grip_to_world_transform
def get_demo_tool_grip_to_world_transform(tool_stage_data, arm):
gripper_data_key = "%s_gripper_tool_frame" % (arm)
demo_tool_grip_pos = tool_stage_data[gripper_data_key]["position"][-1]
demo_tool_grip_orien = tool_stage_data[gripper_data_key]["orientation"][-1]
demo_tool_grip_to_world_transform = juc.trans_rot_to_hmat(demo_tool_grip_pos, demo_tool_grip_orien)
return demo_tool_grip_to_world_transform
from brett2 import ros_utils
import roslib;
roslib.load_manifest('tf')
import tf
import rospy
import sensor_msgs.msg as sm
import numpy as np
from jds_utils import conversions
rospy.init_node("save_point_cloud_data")
listener = tf.TransformListener()
rospy.sleep(.5)
xyzs = []
while True:
response = raw_input("press enter when you're ready. type 'quit' when done")
if 'quit' in response: break
pc = rospy.wait_for_message("/preprocessor/points", sm.PointCloud2)
trans,rot=listener.lookupTransform("base_footprint",pc.header.frame_id, rospy.Time(0))
hmat = conversions.trans_rot_to_hmat(trans,rot)
xyz, rgb = ros_utils.pc2xyzrgb(pc)
xyz = np.squeeze(xyz)
xyz1 = np.c_[xyz, np.ones((len(xyz),1))]
xyz1_base = np.dot(xyz1, hmat.T)
xyzs.append(xyz1_base[:,:3])
print "saving"
np.save("/home/joschu/Data/rope/ropes.npy",np.array(xyzs,dtype=object))
def get_demo_grip_traj_mats(target_stage_data, arm):
gripper_data_key = "%s_gripper_tool_frame" % (arm)
demo_target_grip_traj_xyzs = target_stage_data[gripper_data_key]["position"]
demo_target_grip_traj_oriens = target_stage_data[gripper_data_key]["orientation"]
demo_target_grip_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(demo_target_grip_traj_xyzs, demo_target_grip_traj_oriens)]
return demo_target_grip_traj_mats
def make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None):
arms_used = current_stage_info.arms_used
verb_stage_data = verb_data_accessor.get_demo_data(current_stage_info.stage_name)
if stage_num == 0:
# don't do any extra transformation for the first stage
prev_demo_to_exp_grip_transform_lin_rigid = np.identity(4)
# no special point translation for first stage since no tool yet
special_point_translation = np.identity(4)
elif stage_num > 0:
# make sure that the tool stage only uses one arm (the one with the tool)
assert arms_used in ['r', 'l']
prev_stage_data = verb_data_accessor.get_demo_data(prev_stage_info.stage_name)
prev_demo_pc = prev_stage_data["object_clouds"][prev_stage_info.item]["xyz"]
prev_exp_pc = prev_exp_clouds[0]
prev_demo_pc_down = voxel_downsample(prev_demo_pc, .02)
prev_exp_pc_down = voxel_downsample(prev_exp_pc, .02)
gripper_data_key = "%s_gripper_tool_frame" % (arms_used)
# transform point cloud in base frame to gripper frame
# assume right hand has the tool for now
# use the last pose of the gripper in the stage to figure out the point cloud of the tool in the gripper frame when the tool was grabbed
prev_demo_gripper_pos = prev_stage_data[gripper_data_key]["position"][-1]
prev_demo_gripper_orien = prev_stage_data[gripper_data_key]["orientation"][-1]
prev_demo_gripper_to_base_transform = juc.trans_rot_to_hmat(prev_demo_gripper_pos, prev_demo_gripper_orien)
prev_demo_base_to_gripper_transform = np.linalg.inv(prev_demo_gripper_to_base_transform)
prev_demo_pc_in_gripper_frame = np.array([apply_transform(prev_demo_base_to_gripper_transform, point) for point in prev_demo_pc_down])
# get the new point cloud in the new gripper frame
# prev_exp_pc_in_gripper_frame = [apply_transform(prev_exp_base_to_gripper_transform, point) for point in prev_exp_pc_down]
if to_gripper_frame_func is None:
prev_exp_pc_in_gripper_frame = to_gripper_frame_tf_listener(prev_exp_pc_down, gripper_data_key)
else:
prev_exp_pc_in_gripper_frame = to_gripper_frame_func(prev_exp_pc_down, gripper_data_key)
# get the transformation from the new point cloud to the old point cloud for the previous stage
prev_demo_to_exp_grip_transform = get_tps_transform(prev_demo_pc_in_gripper_frame, prev_exp_pc_in_gripper_frame)
# transforms gripper trajectory point into special point trajectory point
if prev_stage_info.special_point is None:
# if there is no special point, linearize at origin
prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.zeros(3))
# don't do a special point translation if there is no specified special point
special_point_translation = np.identity(4)
else:
prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.array(prev_stage_info.special_point))
# translation from gripper pose in world frame to special point pose in world frame
special_point_translation = jut.translation_matrix(np.array(prev_stage_info.special_point))
if arms_used != 'b':
arms_used_list = [arms_used]
else:
arms_used_list = ['r', 'l']
warped_stage_data = group_to_dict(verb_stage_data) # deep copy it
resp = MakeTrajectoryResponse()
traj = resp.traj
traj.arms_used = arms_used
for arm in arms_used_list:
gripper_data_key = "%s_gripper_tool_frame" % (arm)
# find the special point trajectory before the target transformation
cur_demo_gripper_traj_xyzs = verb_stage_data[gripper_data_key]["position"]
cur_demo_gripper_traj_oriens = verb_stage_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)]
cur_mid_spec_pt_traj_mats = [np.dot(gripper_mat, special_point_translation) for gripper_mat in cur_demo_gripper_traj_mats]
# find the transformation from the new special point to the gripper frame
cur_exp_inv_special_point_transformation = np.linalg.inv(np.dot(prev_demo_to_exp_grip_transform_lin_rigid, special_point_translation))
# save the demo special point traj for plotting
plot_spec_pt_traj = []
for gripper_mat in cur_demo_gripper_traj_mats:
spec_pt_xyz, spec_pt_orien = juc.hmat_to_trans_rot(np.dot(gripper_mat, special_point_translation))
plot_spec_pt_traj.append(spec_pt_xyz)
print 'grip transform:'
print prev_demo_to_exp_grip_transform_lin_rigid
print 'special point translation:'
print special_point_translation
print 'inverse special point translation:'
print cur_exp_inv_special_point_transformation
# find the target transformation for the experiment scene
demo_object_clouds = [verb_stage_data["object_clouds"][obj_name]["xyz"] for obj_name in verb_stage_data["object_clouds"].keys()]
if len(demo_object_clouds) > 1:
raise Exception("i don't know what to do with multiple object clouds")
x_nd = voxel_downsample(demo_object_clouds[0], .02)
y_md = voxel_downsample(cur_exp_clouds[0], .02)
# transformation from old target object to new target object in world frame
cur_demo_to_exp_transform = get_tps_transform(x_nd, y_md)
# apply the target warping transformation to the special point trajectory
cur_mid_spec_pt_traj_xyzs, cur_mid_spec_pt_traj_oriens = [], []
for cur_mid_spec_pt_traj_mat in cur_mid_spec_pt_traj_mats:
cur_mid_spec_pt_traj_xyz, cur_mid_spec_pt_traj_orien = juc.hmat_to_trans_rot(cur_mid_spec_pt_traj_mat)
cur_mid_spec_pt_traj_xyzs.append(cur_mid_spec_pt_traj_xyz)
cur_mid_spec_pt_traj_oriens.append(juc.quat2mat(cur_mid_spec_pt_traj_orien))
cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens = cur_demo_to_exp_transform.transform_frames(np.array(cur_mid_spec_pt_traj_xyzs), np.array(cur_mid_spec_pt_traj_oriens))
plot_warped_spec_pt_traj = cur_exp_spec_pt_traj_xyzs #save the special point traj for plotting
cur_exp_spec_pt_traj_mats = [juc.trans_rot_to_hmat(cur_exp_spec_pt_traj_xyz, mat2quat(cur_exp_spec_pt_traj_orien)) for cur_exp_spec_pt_traj_xyz, cur_exp_spec_pt_traj_orien in zip(cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens)]
# transform the warped special point trajectory back to a gripper trajectory in the experiment
cur_exp_gripper_traj_mats = [np.dot(spec_pt_mat, cur_exp_inv_special_point_transformation) for spec_pt_mat in cur_exp_spec_pt_traj_mats]
warped_stage_data[gripper_data_key]["position"] = []
warped_stage_data[gripper_data_key]["orientation"] = []
for exp_traj_mat in cur_exp_gripper_traj_mats:
warped_pos, warped_orien = juc.hmat_to_trans_rot(exp_traj_mat)
warped_stage_data[gripper_data_key]["position"].append(warped_pos)
warped_stage_data[gripper_data_key]["orientation"].append(warped_orien)
if arm == 'r':
traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
print "poses: ", len(traj.r_gripper_poses.poses)
traj.r_gripper_angles = warped_stage_data["r_gripper_joint"]
traj.r_gripper_poses.header.frame_id = clouds_frame_id
elif arm == 'l':
traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
print "poses: ", len(traj.l_gripper_poses.poses)
traj.l_gripper_angles = warped_stage_data["l_gripper_joint"]
traj.l_gripper_poses.header.frame_id = clouds_frame_id
Globals.handles = []
plot_original_and_warped_demo_and_spec_pt(verb_stage_data, warped_stage_data, plot_spec_pt_traj, plot_warped_spec_pt_traj, 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 jds_utils.conversions import trans_rot_to_hmat,hmat_to_trans_rot
from jds_utils.transformations import euler_from_quaternion
import rospy
import roslib
roslib.load_manifest("tf")
import tf
import numpy as np
class Globals:
listener = None
if rospy.get_name() == "/unnamed":
rospy.init_node("transform_kinect_frames")
Globals.listener = tf.TransformListener()
rospy.sleep(1)
ONIfromCL = trans_rot_to_hmat(*Globals.listener.lookupTransform("/camera_rgb_optical_frame","camera_link", rospy.Time(0)))
GAZfromONI = trans_rot_to_hmat([0, -.15, -.24], [0, 0, 0, 1])
HEADfromGAZ = trans_rot_to_hmat(*Globals.listener.lookupTransform("/head_plate_frame", "/wide_stereo_gazebo_l_stereo_camera_optical_frame",rospy.Time(0)))
HEADfromCL = np.dot(np.dot(HEADfromGAZ, GAZfromONI), ONIfromCL)
trans_final, quat_final = hmat_to_trans_rot(HEADfromCL)
eul_final = euler_from_quaternion(quat_final)
print "translation", trans_final
print "rotation", eul_final
print "%.4f %.4f %.4f %.4f %.4f %.4f %.4f"%(tuple(trans_final) + tuple(quat_final))
