def downsample_by_pose_difference_threshold(images_dir_full_path,
linear_distance_threshold,
rotation_angle_threshold):
"""
Downsamples poses and keeps only those that are sufficiently apart
:param images_dir_full_path:
:type images_dir_full_path:
:param linear_distance_threshold: threshold on the translation, in meters
:type linear_distance_threshold:
:param rotation_angle_threshold: threshold on the angle between the rotations, in degrees
:type rotation_angle_threshold:
:return:
:rtype:
"""
pose_yaml = os.path.join(images_dir_full_path, "pose_data.yaml")
pose_dict = spartanUtils.getDictFromYamlFilename(pose_yaml)
images_dir_temp_path = os.path.join(
os.path.dirname(images_dir_full_path), 'images_temp')
if not os.path.isdir(images_dir_temp_path):
os.makedirs(images_dir_temp_path)
previous_pose_pos = FusionServer.get_numpy_position_from_pose(
pose_dict[0])
previous_pose_quat = FusionServer.get_quaternion_from_pose(
pose_dict[0])
print "Using downsampling by pose difference threshold... "
print "Previously: ", len(pose_dict), " images"
num_kept_images = 0
num_deleted_images = 0
for i in range(0, len(pose_dict)):
single_frame_data = pose_dict[i]
this_pose_pos = FusionServer.get_numpy_position_from_pose(
pose_dict[i])
this_pose_quat = FusionServer.get_quaternion_from_pose(
pose_dict[i])
linear_distance = np.linalg.norm(this_pose_pos - previous_pose_pos)
rotation_distance = spartanUtils.compute_angle_between_quaternions(
this_pose_quat, previous_pose_quat)
if i == 0:
keep_image = True
num_kept_images += 1
elif (linear_distance > linear_distance_threshold) or (
np.rad2deg(rotation_distance) > rotation_angle_threshold):
previous_pose_pos = this_pose_pos
previous_pose_quat = this_pose_quat
num_kept_images += 1
else:
# delete pose from forward kinematics
del pose_dict[i]
continue
# if we have gotten here, then move the images over to the new directory
rgb_filename = os.path.join(
images_dir_full_path, single_frame_data['rgb_image_filename'])
rgb_filename_temp = os.path.join(
images_dir_temp_path, single_frame_data['rgb_image_filename'])
shutil.move(rgb_filename, rgb_filename_temp)
depth_filename = os.path.join(
images_dir_full_path,
single_frame_data['depth_image_filename'])
depth_filename_temp = os.path.join(
images_dir_temp_path,
single_frame_data['depth_image_filename'])
shutil.move(depth_filename, depth_filename_temp)
# # delete pose from posegraph
# del posegraph_list[i-num_deleted_images]
# num_deleted_images += 1
# write downsamples pose_data.yaml (forward kinematics)
spartanUtils.saveToYaml(
pose_dict, os.path.join(images_dir_temp_path, 'pose_data.yaml'))
# remove old images
shutil.move(os.path.join(images_dir_full_path, 'camera_info.yaml'),
os.path.join(images_dir_temp_path, 'camera_info.yaml'))
shutil.rmtree(images_dir_full_path)
print "renaming %s to %s " % (images_dir_temp_path,
images_dir_full_path)
# rename temp images to images
os.rename(images_dir_temp_path, images_dir_full_path)
print "After: ", num_kept_images, " images"
def test_cartesian_space_plan(self):
"""
Test the cartesian space plan
"""
self._start_ros_node_and_wait_for_sim()
above_table_pre_grasp = [
0.04486168762069299, 0.3256606458812486, -0.033502080520812445,
-1.5769091802934694, 0.05899249087322813, 1.246379583616529,
0.38912999977004026
]
targetPosition = above_table_pre_grasp
robotService = rosUtils.RobotService.makeKukaRobotService()
success = robotService.moveToJointPosition(targetPosition, timeout=5)
self.assertTrue(
success, msg="RobotService MoveToJointPosition returned failure ")
# check that we actually reached the target position
lastRobotJointPositions = self._robotSubscriber.get_position_vector_from_joint_names(
self.kuka_joint_names)
reached_target_position = np.linalg.norm(
np.array(targetPosition) -
np.array(lastRobotJointPositions[0:7])) < 0.1
# now call the cartesian space plan service
client = actionlib.SimpleActionClient(
"plan_runner/CartesianTrajectory",
robot_msgs.msg.CartesianTrajectoryAction)
print "waiting for server"
client.wait_for_server()
print "connected to server"
goal = make_cartesian_trajectory_goal_world_frame()
goal.gains.append(make_cartesian_gains_msg())
goal.force_guard.append(make_force_guard_msg())
print "sending goal"
client.send_goal(goal)
rospy.loginfo("waiting for CartesianTrajectory action result")
client.wait_for_result()
result = client.get_result()
rospy.sleep(3) # wait for controller to settle
success = (result.status.status ==
robot_msgs.msg.PlanStatus.FINISHED_NORMALLY)
print "result:", result
self.assertTrue(success, msg="PlanStatus was not FINISHED_NORMALLY")
# check the position
# check that desired position matches actual
self.setupTF()
iiwa_link_ee_to_world = self.get_ee_frame_pose()
pos_actual = np.array(
rosUtils.pointMsgToList(
iiwa_link_ee_to_world.transform.translation))
pos_desired = np.array(
rosUtils.pointMsgToList(goal.trajectory.xyz_points[-1].point))
quat_actual = np.array(
rosUtils.quatMsgToList(iiwa_link_ee_to_world.transform.rotation))
quat_desired = np.array(
rosUtils.quatMsgToList(goal.trajectory.quaternions[0]))
pos_tol = 0.01 # within 5 cm
orientation_tol = 10 # within 5 degrees
pos_error = np.linalg.norm(pos_actual - pos_desired)
orientation_error_deg = 180 / np.pi * spartan_utils.compute_angle_between_quaternions(
quat_actual, quat_desired)
print "pos_error:\n", pos_error
print "orientation error:\n", orientation_error_deg
self.assertTrue(pos_error < pos_tol,
msg="position error was above tolerance")
self.assertTrue(orientation_error_deg < orientation_tol,
msg="orientation error was above tolerance")