def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
hmats = [rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz]) for (xyz, quat) in zip(xyzs, quats)]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def similar_trajectories(traj1, traj2):
if len(traj1) != len(traj2):
return False, "trajectory lengths don't match"
for index, (traj1_pt, traj2_pt) in enumerate(zip(traj1, traj2)):
traj1_pt_xyz, traj1_pt_rot = juc.hmat_to_trans_rot(traj1_pt)
traj2_pt_xyz, traj2_pt_rot = juc.hmat_to_trans_rot(traj2_pt)
xyz_dist = euclidean_dist(traj1_pt_xyz, traj2_pt_xyz)
rot_dist = rot_distance(traj1_pt_rot, traj2_pt_rot)
if xyz_dist > XYZ_TOLERANCE or rot_dist > ROT_TOLERANCE:
error_msg = "Incorrect point (index %i): { traj1: %s %s, traj2: %s %s }, { xyz_dist: %f, rot_dist: %f }" % (index, str(traj1_pt_xyz), str(traj1_pt_rot), str(traj2_pt_xyz), str(traj2_pt_rot), xyz_dist, rot_dist)
return False, error_msg
return True, "matching"
def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [
rave.quatSlerp(quat_start, quat_end, t)
for t in np.linspace(0, 1, n_steps)
]
hmats = [
rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz])
for (xyz, quat) in zip(xyzs, quats)
]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def similar_trajectories(traj1, traj2):
if len(traj1) != len(traj2):
return False, "trajectory lengths don't match"
for index, (traj1_pt, traj2_pt) in enumerate(zip(traj1, traj2)):
traj1_pt_xyz, traj1_pt_rot = juc.hmat_to_trans_rot(traj1_pt)
traj2_pt_xyz, traj2_pt_rot = juc.hmat_to_trans_rot(traj2_pt)
xyz_dist = euclidean_dist(traj1_pt_xyz, traj2_pt_xyz)
rot_dist = rot_distance(traj1_pt_rot, traj2_pt_rot)
if xyz_dist > XYZ_TOLERANCE or rot_dist > ROT_TOLERANCE:
error_msg = "Incorrect point (index %i): { traj1: %s %s, traj2: %s %s }, { xyz_dist: %f, rot_dist: %f }" % (
index, str(traj1_pt_xyz), str(traj1_pt_rot), str(traj2_pt_xyz),
str(traj2_pt_rot), xyz_dist, rot_dist)
return False, error_msg
return True, "matching"
def lookupTransform(self, to_frame, from_frame, *args):
if not to_frame.startswith("/"):
to_frame = "/" + to_frame
if not from_frame.startswith("/"):
to_frame = "/" + from_frame
hmat = self.transformer.lookup_transform_mat(to_frame, from_frame)
return conversions.hmat_to_trans_rot(hmat)
def transform_demo_with_fingertips(f, demo):
"""
demo has a bunch of fields with arrays
this function uses the Transformation f to transform some of the fields of it using f
"""
warped_demo = {}
for lr in "lr":
if "%s_gripper_tool_frame" % lr in demo:
_, ori = f.transform_frames(
demo["%s_gripper_tool_frame" % lr]["position"],
quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
xyz_fingertip0 = f.transform_points(
demo["%s_gripper_l_finger_tip_link" % lr]["position"])
xyz_fingertip1 = f.transform_points(
demo["%s_gripper_r_finger_tip_link" % lr]["position"])
closing_only = get_gripper_closing_only_times(demo, lr)
assert len(closing_only) == len(ori)
tool_xyzs = []
tool_quats = []
tool_angles = []
print calc_hand_pose(xyz_fingertip0[0], xyz_fingertip1[0],
ori[0])[0][:3, :3]
first, last_xyz, last_quat = True, None, None
for (pos0, pos1, o, cl) in zip(xyz_fingertip0, xyz_fingertip1, ori,
closing_only):
hmat, ang = calc_hand_pose(pos0, pos1, o)
# if the gripper is closing and nothing else is moving,
# don't change anything but the gripper angle
if SPECIAL_GRIPPER_CLOSE and cl and not first:
xyz, quat = last_xyz, last_quat
else:
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
last_xyz, last_quat = xyz, quat
first = False
warped_demo["%s_gripper_tool_frame" % lr] = {}
warped_demo["%s_gripper_l_finger_tip_link" % lr] = {}
warped_demo["%s_gripper_l_finger_tip_link" %
lr]["position"] = xyz_fingertip1
warped_demo["%s_gripper_r_finger_tip_link" % lr] = {}
warped_demo["%s_gripper_r_finger_tip_link" %
lr]["position"] = xyz_fingertip0
warped_demo["%s_gripper_tool_frame" %
lr]["position"] = np.array(tool_xyzs)
warped_demo["%s_gripper_tool_frame" %
lr]["orientation"] = np.array(tool_quats)
warped_demo["%s_gripper_joint" % lr] = np.array(tool_angles)
return warped_demo
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 plot_demo_and_warped_tool_spec_pt(spec_pt_in_grip, tool_stage_data, demo_to_exp_tool_transform, arm):
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)
plot_spec_pts(np.array([demo_spec_pt_in_world]), (1,0,0,1))
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)
plot_spec_pts(np.array([warped_spec_pt_in_world_trans]), (0,1,0,1))
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 plot_demo_and_warped_tool_spec_pt(spec_pt_in_grip, tool_stage_data, demo_to_exp_tool_transform, arm):
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)
plot_spec_pts(np.array([demo_spec_pt_in_world]), (1,0,0,0.5))
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)
plot_spec_pts(np.array([warped_spec_pt_in_world_trans]), (0,1,0,0.5))
def remove_lower_poses(current_pos, gripper_poses, gripper_angles):
xyz, quat = juc.hmat_to_trans_rot(current_pos)
z_floor = xyz[2]
low_index = 0
for pose in gripper_poses:
if pose.position.z >= z_floor:
break
low_index += 1
removed_lower_poses = gripper_poses[low_index:]
updated_angles = gripper_angles[low_index:]
return removed_lower_poses, updated_angles
def remove_lower_poses(current_pos, gripper_poses, gripper_angles):
xyz, quat = juc.hmat_to_trans_rot(current_pos)
z_floor = xyz[2]
low_index = 0
for pose in gripper_poses:
if pose.position.z >= z_floor:
break
low_index += 1
removed_lower_poses = gripper_poses[low_index:]
updated_angles = gripper_angles[low_index:]
return removed_lower_poses, updated_angles
def transform_demo_with_fingertips(f, demo):
"""
demo has a bunch of fields with arrays
this function uses the Transformation f to transform some of the fields of it using f
"""
warped_demo = {}
for lr in "lr":
if "%s_gripper_tool_frame"%lr in demo:
_, ori = f.transform_frames(demo["%s_gripper_tool_frame"%lr]["position"], quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
xyz_fingertip0 = f.transform_points(demo["%s_gripper_l_finger_tip_link"%lr]["position"])
xyz_fingertip1 = f.transform_points(demo["%s_gripper_r_finger_tip_link"%lr]["position"])
closing_only = get_gripper_closing_only_times(demo, lr)
assert len(closing_only) == len(ori)
tool_xyzs = []
tool_quats = []
tool_angles = []
print calc_hand_pose(xyz_fingertip0[0], xyz_fingertip1[0], ori[0])[0][:3,:3]
first, last_xyz, last_quat = True, None, None
for (pos0, pos1, o, cl) in zip(xyz_fingertip0, xyz_fingertip1, ori, closing_only):
hmat, ang = calc_hand_pose(pos0, pos1, o)
# if the gripper is closing and nothing else is moving,
# don't change anything but the gripper angle
if SPECIAL_GRIPPER_CLOSE and cl and not first:
xyz, quat = last_xyz, last_quat
else:
xyz, quat = conversions.hmat_to_trans_rot(hmat)
tool_xyzs.append(xyz)
tool_quats.append(quat)
tool_angles.append(ang)
last_xyz, last_quat = xyz, quat
first = False
warped_demo["%s_gripper_tool_frame"%lr]={}
warped_demo["%s_gripper_l_finger_tip_link"%lr]={}
warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"]=xyz_fingertip1
warped_demo["%s_gripper_r_finger_tip_link"%lr]={}
warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]=xyz_fingertip0
warped_demo["%s_gripper_tool_frame"%lr]["position"] = np.array(tool_xyzs)
warped_demo["%s_gripper_tool_frame"%lr]["orientation"] = np.array(tool_quats)
warped_demo["%s_gripper_joint"%lr] = np.array(tool_angles)
return warped_demo
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 testTransforms (tfms, child_frames, parent_frames, time = 25):
"""
Basic code to test transforms. Visualize on RViz or something.
"""
br = tf.TransformBroadcaster()
rate = rospy.Rate(0.2)
initTime = rospy.Time.now()
totTime = rospy.Duration(time)
if not time:
foreverCheck = True
else:
foreverCheck = False
while foreverCheck or (rospy.Time.now() - initTime < totTime):
for tfm, child_frame, parent_frame in zip(tfms, child_frames,parent_frames):
(trans, rot) = conv.hmat_to_trans_rot(tfm)
br.sendTransform(trans, rot,
rospy.Time.now(),
child_frame,
parent_frame)
rate.sleep()
def rot_distance(quat1, quat2):
quat1_mat = jut.quaternion_matrix(quat1)
quat2_mat = jut.quaternion_matrix(quat2)
diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
return euclidean_dist(diff_quat, [0, 0, 0, 1])
def hmat_to_ros_transform(hmat):
(x, y, z), (xx, yy, zz, ww) = conversions.hmat_to_trans_rot(hmat)
return gm.Transform(translation=gm.Vector3(x, y, z),
rotation=gm.Quaternion(xx, yy, zz, ww))
def lookupTransform(self, to_frame, from_frame, *args):
if not to_frame.startswith('/'): to_frame = '/' + to_frame
if not from_frame.startswith('/'): to_frame = '/' + from_frame
hmat = self.transformer.lookup_transform_mat(to_frame, from_frame)
return conversions.hmat_to_trans_rot(hmat)
def extract_kinematics_from_bag(bag, link_names):
"""
Input bagfile has the following topics: /tf, /joint_states, /joy, /xxx/points
Output dictionary has a bunch of arrays with kinematics info, e.g.
/r_gripper_tool_frame/position
orientation
hmat
/joint_states/position
orientation
etc.
"""
rr = RosToRave.create()
robot = rr.robot
links = [robot.GetLink(name) for name in link_names]
l_joint_inds = robot.GetManipulator("leftarm").GetArmIndices()
r_joint_inds = robot.GetManipulator("rightarm").GetArmIndices()
l_gripper_inds = [robot.GetJointIndex("l_gripper_joint")]
r_gripper_inds = [robot.GetJointIndex("r_gripper_joint")]
out = {}
for link_name in link_names:
out[link_name] = {}
out[link_name]["position"] = []
out[link_name]["orientation"] = []
out[link_name]["hmat"] = []
out["joint_states"] = {}
out["joint_states"]["position"] = []
out["leftarm"] = []
out["rightarm"] = []
out["l_gripper_joint"] = []
out["r_gripper_joint"] = []
out["time"] = []
first_message = True
for (topic, msg, t) in bag.read_messages(topics=['/joint_states']):
t = t.to_sec()
if first_message:
rr.init(msg)
out["joint_states"]["name"] = msg.name
prev_positions = np.ones(len(msg.position)) * 100
prev_t = -np.inf
first_message = False
if t - prev_t > MIN_TIME and np.any(
np.abs(np.array(msg.position) -
prev_positions) > MIN_JOINT_CHANGE):
rr.update(msg)
out["joint_states"]["position"].append(msg.position)
out["time"].append(t)
for (link_name, link) in zip(link_names, links):
hmat = link.GetTransform()
xyz, quat = conversions.hmat_to_trans_rot(hmat)
out[link_name]["position"].append(xyz)
out[link_name]["orientation"].append(quat)
out[link_name]["hmat"].append(hmat)
robot.SetActiveDOFs(l_joint_inds)
out["leftarm"].append(robot.GetActiveDOFValues())
robot.SetActiveDOFs(r_joint_inds)
out["rightarm"].append(robot.GetActiveDOFValues())
robot.SetActiveDOFs(l_gripper_inds)
out["l_gripper_joint"].append(robot.GetActiveDOFValues()[0])
robot.SetActiveDOFs(r_gripper_inds)
out["r_gripper_joint"].append(robot.GetActiveDOFValues()[0])
prev_t = t
prev_positions = np.array(msg.position)
out = convert_lists_to_arrays(out)
return out
def rot_distance(quat1, quat2):
quat1_mat = jut.quaternion_matrix(quat1)
quat2_mat = jut.quaternion_matrix(quat2)
diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
return (sum([x**2 for x in jut.euler_from_quaternion(diff_quat)]))**0.5
def rot_distance(quat1, quat2):
quat1_mat = jut.quaternion_matrix(quat1)
quat2_mat = jut.quaternion_matrix(quat2)
diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
return euclidean_dist(diff_quat, [0, 0, 0, 1])
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 extract_kinematics_from_bag(bag, link_names):
"""
Input bagfile has the following topics: /tf, /joint_states, /joy, /xxx/points
Output dictionary has a bunch of arrays with kinematics info, e.g.
/r_gripper_tool_frame/position
orientation
hmat
/joint_states/position
orientation
etc.
"""
rr = RosToRave.create()
robot = rr.robot
links = [robot.GetLink(name) for name in link_names]
l_joint_inds = robot.GetManipulator("leftarm").GetArmIndices()
r_joint_inds = robot.GetManipulator("rightarm").GetArmIndices()
l_gripper_inds = [robot.GetJointIndex("l_gripper_joint")]
r_gripper_inds = [robot.GetJointIndex("r_gripper_joint")]
out = {}
for link_name in link_names:
out[link_name] = {}
out[link_name]["position"] = []
out[link_name]["orientation"] = []
out[link_name]["hmat"] = []
out["joint_states"] = {}
out["joint_states"]["position"] = []
out["leftarm"] = []
out["rightarm"] = []
out["l_gripper_joint"] = []
out["r_gripper_joint"] = []
out["time"] = []
first_message = True
for (topic, msg, t) in bag.read_messages(topics=['/joint_states']):
t = t.to_sec()
if first_message:
rr.init(msg)
out["joint_states"]["name"] = msg.name
prev_positions = np.ones(len(msg.position))*100
prev_t = -np.inf
first_message = False
if t - prev_t > MIN_TIME and np.any(np.abs(np.array(msg.position) - prev_positions) > MIN_JOINT_CHANGE):
rr.update(msg)
out["joint_states"]["position"].append(msg.position)
out["time"].append(t)
for (link_name, link) in zip(link_names,links):
hmat = link.GetTransform()
xyz, quat = conversions.hmat_to_trans_rot(hmat)
out[link_name]["position"].append(xyz)
out[link_name]["orientation"].append(quat)
out[link_name]["hmat"].append(hmat)
robot.SetActiveDOFs(l_joint_inds)
out["leftarm"].append(robot.GetActiveDOFValues())
robot.SetActiveDOFs(r_joint_inds)
out["rightarm"].append(robot.GetActiveDOFValues())
robot.SetActiveDOFs(l_gripper_inds)
out["l_gripper_joint"].append(robot.GetActiveDOFValues()[0])
robot.SetActiveDOFs(r_gripper_inds)
out["r_gripper_joint"].append(robot.GetActiveDOFValues()[0])
prev_t = t
prev_positions = np.array(msg.position)
out = convert_lists_to_arrays(out)
return out
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 hmat_to_ros_transform(hmat):
(x, y, z), (xx, yy, zz, ww) = conversions.hmat_to_trans_rot(hmat)
return gm.Transform(translation=gm.Vector3(x, y, z), rotation=gm.Quaternion(xx, yy, zz, ww))
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))