diff --git a/.gitignore b/.gitignore index 3665b833..7e58811c 100644 --- a/.gitignore +++ b/.gitignore @@ -153,4 +153,5 @@ models/main_models/rt1_dataset/* models/main_models/traj_rollouts/* models/main_models/run_rt1_main.sh models/main_models/rt1_ft_env -/models/main_models/alfred/results/*/*.pkl \ No newline at end of file +/modespot/ +Trajectories/ diff --git a/spot/actions.py b/spot/actions.py new file mode 100644 index 00000000..4df58f7a --- /dev/null +++ b/spot/actions.py @@ -0,0 +1,1961 @@ +import configparser +import argparse +import os +import pickle +from tqdm import tqdm +import numpy as np +from collections import defaultdict +from queue import PriorityQueue +from PIL import Image +import clip +import torch +import time +import json +from google.protobuf import wrappers_pb2 + +from bosdyn.api import arm_command_pb2, robot_command_pb2, synchronized_command_pb2 + +from skill_utils import get_best_clip_vild_dirs + +from spot_utils.utils import pixel_to_vision_frame, pixel_to_vision_frame_depth_provided, arm_object_grasp, grasp_object, open_gripper +from spot_utils.generate_pointcloud import make_pointcloud +from vild.vild_utils import visualize_boxes_and_labels_on_image_array, plot_mask +from clipseg.clipseg_utils import ClipSeg +import matplotlib.pyplot as plt +from matplotlib import patches +from torchvision import transforms + +from bosdyn.api import arm_command_pb2, robot_command_pb2, synchronized_command_pb2, trajectory_pb2 +from bosdyn.client import math_helpers +from bosdyn.util import seconds_to_duration + +import cv2 + +# import open3d as o3d + +from bosdyn.client.image import ImageClient +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.lease import LeaseKeepAlive, LeaseClient +from bosdyn.client.recording import GraphNavRecordingServiceClient +from bosdyn.client import ResponseError, RpcError +from bosdyn.util import duration_to_seconds + +import bosdyn.api.gripper_command_pb2 +import bosdyn.client +import bosdyn.client.lease +import bosdyn.client.util + +from bosdyn.api import geometry_pb2 +from bosdyn.api import basic_command_pb2 + +from bosdyn.client.frame_helpers import VISION_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME, get_a_tform_b, GRAV_ALIGNED_BODY_FRAME_NAME, ODOM_FRAME_NAME +from bosdyn.client.robot_command import (RobotCommandBuilder, RobotCommandClient, + block_until_arm_arrives, blocking_stand) + +from constrained_manipulation_helper import construct_drawer_task +import pandas as pd + +from sentence_transformers import SentenceTransformer, util +import spacy + +from spot_utils.move_spot_to import simple_move_to + +import pandas as pd +from spot_utils.move_spot_to import move_to +from PIL import Image + +from recording_command_line import RecordingInterface +from graph_nav_command_line import GraphNavInterface + +import copy + +def fill_action_template(program_line_args): + + action_templates_to_english = {'pick up': '{} the {}', 'put down': '{} the {} on the {}', 'walk to': '{} the {}', 'open': '{} the {}', 'close': '{} the {}', 'stand up': '{}', 'sit down': '{}', 'look at': '{} the {}' } + + + template = action_templates_to_english[program_line_args[0]] + + + if program_line_args[0] == 'put down': + template = template.format(program_line_args[0], program_line_args[1], program_line_args[2]) + elif program_line_args[0] not in set(['sit down', 'stand up']): + template = template.format(program_line_args[0], program_line_args[1]) + else: + template = template.format(program_line_args[0]) + + return template + +def continue_block(): + + select = None + + while select is None: + arg = input('continue? [y/n]: ') + + if arg.strip().lower() == 'y': + select = 'y' + elif arg.strip().lower() == 'n': + select = 'n' + else: + print('select a valid option\n') + + return select + + +class Action(): + def __init__(self, hostname, scene_num, scene_path='./scene_graphs/scene_1.json', start_location='origin', with_robot = False, dist_threshold=0.7): + + self.with_robot = with_robot + + if self.with_robot: + self.hostname = hostname + sdk = bosdyn.client.create_standard_sdk('ActionTester') + self.robot = sdk.create_robot(hostname) + + try: + bosdyn.client.util.authenticate(self.robot) + self.robot.start_time_sync(1) + except RpcError as err: + LOGGER.error("Failed to communicate with robot: %s" % err) + return False + + bosdyn.client.util.authenticate(self.robot) + + + self.sources = ['hand_depth_in_hand_color_frame', 'hand_color_image'] + self.default_timeout = 20 + + self.robot_id = self.robot.get_id() + + + # Time sync is necessary so that time-based filter requests can be converted + self.robot.time_sync.wait_for_sync() + + assert not self.robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ + "such as the estop SDK example, to configure E-Stop." + + self.robot_state_client = self.robot.ensure_client( + bosdyn.client.robot_state.RobotStateClient.default_service_name) + + self.lease_client = self.robot.ensure_client( + LeaseClient.default_service_name) + self.lease_keepalive = LeaseKeepAlive( + self.lease_client, must_acquire=True, return_at_exit=True) + + self.image_client = self.robot.ensure_client( + ImageClient.default_service_name) + self.manipulation_api_client = self.robot.ensure_client( + ManipulationApiClient.default_service_name) + self.robot_command_client = self.robot.ensure_client( + RobotCommandClient.default_service_name) + + self.robot.power_on(timeout_sec=self.default_timeout) + assert self.robot.is_powered_on(), "Robot power on failed." + + + #fields for recording a scene graph of environment + self.client_metadata = GraphNavRecordingServiceClient.make_client_metadata( + session_name='llm_planning', client_username=self.robot._current_user, client_id='RecordingClient', + client_type='Python SDK') + + self.clip_model, self.clip_preprocess = clip.load('ViT-B/32') + self.clipseg_model = ClipSeg(threshold_mask=True, threshold_value=0.55, depth_product=False) + self.use_clipseg_binary = True + + # get spot to stand first + blocking_stand(self.robot_command_client, + timeout_sec=self.default_timeout) + + # pointcloud parameters + self.original_pcd = None + self.dist_threshold = dist_threshold + + # path for pointcloud generation + self.data_path = './data/default_data/' + self.graph_path = './graphs' + + #os.makedirs(self.graph_path, exist_ok=True) + + self.pose_data_fname = "pose_data.pkl" + self.embedding_data_fname = 'clip_embeddings.pkl' + self.pointcloud_fname = "pointcloud.pcd" + #show time taken to process image for grabbing with ViLD vs CLIPSeg + self.show_time = False + + # Path for images + self.img_dir_root_path = "./data/" + self.img_dir_name = "default_data" + self.img_dir_path = self.img_dir_root_path + self.img_dir_name + + self.similarity_model = SentenceTransformer( + 'sentence-transformers/all-roberta-large-v1') + self.object_skills = ["pick up", "drop down", + "walk to", "open", "close",] + self.robot_skills = ["stand up", "sit down"] + self.nlp = spacy.load("en_core_web_sm") + + + + + #NOTE: used for interaction with planner + self.use_translated = True + self.dist_epsilon = 2.0 + + self.manual_images = True + self.device = 'cuda:0' if torch.cuda.is_available() else 'cpu' + + with open(scene_path) as scene_graph: + graph_scene = json.load(scene_graph) + + + #NOTE: ROBOT STATE VARIABLES USED FOR CHECKING PRECONDITON FAILURE + self.robot_standing = graph_scene['spot_state']['standing'] + self.robot_holding = graph_scene['spot_state']['holding'] + self.robot_holding_object = graph_scene['spot_state']['holding_object'] + self.robot_location = graph_scene['spot_state']['location'] + + self.robot_misaligned = graph_scene['spot_state']['misaligned'] + + self.saved_waypoints = set() #track waypoints that are manually created + + self.object_states = self._parse_state_dictionary(copy.deepcopy(graph_scene['object_states'])) + self.room_state = self._parse_state_dictionary(graph_scene['room_state']) + self.id_to_name = self._parse_id_to_name(copy.deepcopy(graph_scene['object_states'])) + + #store the initial states too + self.init_object_states = copy.deepcopy(self.object_states) + self.init_room_state = copy.deepcopy(self.room_state) + self.init_spot_state = copy.deepcopy(graph_scene['spot_state']) + + self.with_pause = True + + def _get_curr_spot_state(self): + return {'standing': self.robot_standing, 'holding': self.robot_holding, 'holding_object': self.robot_holding_object, 'location': self.robot_location} + + def _parse_id_to_name(self, dict_list): + + output_dict = {} + + for item in dict_list: + new_key = item['id'] + new_val = item['name'] + + output_dict[new_key] = new_val + + return output_dict + + + def _parse_state_dictionary(self, dict_list): + + output_dict = {} + + for item in dict_list: + new_key = item['name'] + + item.pop('name') + + output_dict[new_key] = item + + return output_dict + + def _get_location_object_room(self, location_object): + + room = location_object + + all_rooms = set(self.room_state.keys()) + + while room not in all_rooms: + room = self.object_states[room]['location'] + + return room + + def _get_pose_data_for_closest_image(self, text_prompt): + + #load all saved pose data into an array + with open(self.data_path+self.pose_data_fname, 'rb') as f: + pose_data = pickle.load(f) + + + #normalize clip image embeddings + with open(self.data_path+ self.embedding_data_fname, 'rb') as f: + clip_embeddings = pickle.load(f) + + clip_embeddings /= clip_embeddings.norm(dim=-1, keepdim=True) + + + #get clip text embeddings for the prompt + tokenized_text_prompt = clip.tokenize([text_prompt]).to(self.device) + with torch.no_grad(): + text_features = self.clip_model.encode_text(text_prompt) + + + clip_scores = (100.0 * clip_embeddings @ text_features.T).softmax(dim=-1) + best_clip_score, chosen_index = similarity[0].topk(1) + + #fetch pose data for that image and return along with clip score + chosen_pose_data = pose_data[chosen_index] + + return chosen_pose_data, best_clip_score, chosen_idx + + + + def _check_precondition_error(self, skill, obj, loc, text_prompt): + + precondition_error = False + error_info = None + error_params = {} + error_msg = None + skippable = True + + #get robot current position + # frame_tree_snapshot = self.robot.get_frame_tree_snapshot() + # vision_tform_hand = get_a_tform_b(frame_tree_snapshot, "vision", "hand") + + # graphnav_localization_state = self.graphnav_interface._graph_nav_client.get_localization_state() + # seed_tform_body = graphnav_localization_state.localization.seed_tform_body + # seed_tform_body = bosdyn.client.math_helpers.SE3Pose(seed_tform_body.position.x, seed_tform_body.position.y, seed_tform_body.position.z, seed_tform_body.rotation) + + # seed_tform_hand = seed_tform_body * body_tform_hand + + # robot_position = [seed_tform_hand.position.x, seed_tform_hand.position.y, seed_tform_hand.position.z] + + #precondition error 4.5: not next to the object when trying to pick up, put down or look at + # closest_pose_data, __, __ = _get_pose_data_for_closest_image(text_prompt) + + #get the natural language variant of skill, obj, loc + lang_loc = loc.replace('_',' ') + lang_skill = skill.replace('_',' ') + lang_obj = obj.replace('_',' ') + + #find room within which obj is contained + obj_room = self._get_location_object_room(obj) + lang_obj_room = obj_room.replace('_',' ') + + #find room in which spot is currently contained + spot_room = self._get_location_object_room(self.robot_location) + + #robot is already sitting + if ((skill=="sit_down") and not self.robot_standing): + precondition_error = True + error_info = 'I am already sitting down' + skippable = True + error_params = {'type':'already_sitting', 'character':'character'} + + #robot already standing up + elif ((skill=="stand_up") and self.robot_standing): + precondition_error = True + error_info = 'I am already standing up' + skippable = True + error_params = {'type':'already_standing', 'character':'character'} + + #object is not openable, or holdable + elif ((skill=="close") and self.object_states[obj]['openable'] is False) or ((skill=="open") and self.object_states[obj]['openable'] is False) or ((skill=='pick_up') and self.object_states[obj]['holdable'] is False): + precondition_error = True + error_info = 'cannot {} the {} '.format(skill, obj) + skippable = False + error_params = {'type': 'invalid_action', 'action': lang_skill, 'obj': lang_obj} + + #object cannot be put down since location is not a container + elif ((skill=="put_down") and self.object_states[loc]['container'] is False): + precondition_error = True + error_info = 'cannot {} the {} on {}'.format(skill, obj, loc) + skippable = False + error_params = {'type': 'invalid_action', 'action': lang_skill, 'obj': lang_obj + ' on ' + lang_loc} + + #robot attempts to walk to something it is already holding + elif ((skill=="walk_to") and self.robot_holding_object==obj): + precondition_error = True + error_info = 'I am already holding the {}'.format(obj) + skippable = True + error_params = {'type': 'already_holding', 'character': 'character', 'obj': lang_obj} + + #robot walks, picks up, puts down without stading up first + elif ((skill=="walk_to" or skill=="pick_up" or skill=="put_down" or skill=="open" or skill=="close") and not self.robot_standing): + precondition_error = True + error_info = 'I am not standing up' + skippable = False + error_params = {'type':'missing_step', 'character':'character', 'subtype':'not_standing_up'} + + #robot attempts pick up, put down, open, close after misaligning wrt graphnav + elif ((skill=="pick_up" or skill=="put_down" or skill=="open" or skill=="close") and self.robot_misaligned): + precondition_error = True + error_info = 'I am not near the {}'.format(obj) if skill!="put_down" else 'I am not near the {}'.format(loc) + + skippable = False + error_params = {'type':'proximity', 'character':'character', 'obj': obj if skill!= "put_down" else loc} + + #robot walks though it is already near location + elif ((skill=="walk_to") and self.robot_location == obj): + precondition_error = True + error_info = 'I am already near the {}'.format(obj) + skippable = True + error_params = {'type': 'missing_step', 'character': 'character', 'subtype': 'already near the {}'.format(lang_obj)} + + #robot attempts to walk through place obstructed by doors + elif ((skill=="walk_to") and obj_room in self.room_state[spot_room]['neighbour_doors'] and (self.object_states[self.id_to_name[self.room_state[spot_room]['neighbour_doors'][obj_room]]]['open'] is False)): + precondition_error = True + error_info = 'The door between {} and {} is closed'.format(spot_room, obj_room) + skippable = False + error_params = {'type': 'door_closed', 'target_room': lang_obj_room} + + #robot attempts to pick but already holding + elif ((skill=="pick_up") and self.robot_holding): + precondition_error= True + error_info = 'I am already holding something, so cannot {} {}'.format(skill, obj) + skippable = False + error_params = {'type':'hands_full', 'obj':lang_obj, 'action': lang_skill} + + #robot attempts to pick but object inside a closed container + elif ((skill=="pick_up") and 'contained_in' in self.object_states[obj] and self.object_states[self.id_to_name[self.object_states[obj]['contained_in']]]['openable'] and (self.object_states[self.id_to_name[self.object_states[obj]['contained_in']]]['open'] is False)): + precondition_error = True + error_info = 'the {} is inside something else'.format(obj) + skippable = False + error_params = {'type':'internally_contained', 'obj':lang_obj} + + #robot attempts to put down but does not have the object or has the wrong object + elif ((skill=="put_down") and (self.robot_holding is False or self.robot_holding_object!=obj)): + precondition_error = True + error_info = 'I am not holding the {}'.format(obj) + skippable = False + error_params = {'type':'not_holding', 'character':'character', 'obj': lang_obj} + + #robot attempts to pick up, open or close something but is not at location + elif ((skill=="pick_up" or skill=="open" or skill=="close") and self.robot_location!=obj): + precondition_error = True + error_info = 'I am not near the {}'.format(obj) + skippable = False + error_params = {'type':'proximity', 'character':'character', 'obj':lang_obj} + + + #robot attempts to put down but is not near location + elif ((skill=="put_down") and self.robot_location!=loc): + precondition_error = True + error_info = 'I am not near the {}'.format(loc) + skippable = False + error_params = {'type':'proximity', 'character':'character', 'obj':lang_loc} + + #robot attempts pick up, put down or look at but is too far away + # elif ((skill=="pick_up" or skill=="put_down" or skill=="look_at") and np.sqrt(np.sum(np.squared(np.array(robot_position) - np.array(closest_pose_data['position']))))>= self.dist_epsilon): + # precondition_error = True + # error_info = 'I am not near the {}'.format(obj) + # skippable = False + # error_params = {'type':'proximity', 'character':'character', 'obj':lang_obj} + + #robot attempts to open or close something whilst holding an object + elif ((skill=="open" or skill=="close") and self.robot_holding is True): + precondition_error = True + error_info = 'I am already holding something, so cannot {} {}'.format(skill, obj) + skippable = False + error_params = {'type': 'hands_full', 'obj': lang_obj, 'action': lang_skill} + + #robot opens something that is already open + elif ((skill=="open") and self.object_states[obj]['openable'] and self.object_states[obj]['open'] is True): + precondition_error = True + error_info = 'the {} is already open'.format(obj) + skippable = True + error_params = {'type':'unflipped_boolean_state', 'obj':lang_obj, 'error_state': 'open'} + + #robot closes something that is already closed + elif ((skill=="close") and object_states[obj]['openable'] and self.object_states[obj]['open'] is False): + precondition_error = True + error_info = 'the {} is already closed'.format(obj) + skippable = True + error_params = {'type':'unflipped_boolean_state', 'obj': lang_obj, 'error_state': 'closed'} + + if precondition_error: + task_script = '{} {}'.format(skill, obj) if loc is '' else '{} {} on {}'.format(skill, obj, loc) + + error_msg = ' cannot {}. '.format(task_script) + (error_info[0].upper() + error_info[1:]).replace('I am', ' is') + + return precondition_error, error_params, error_msg, skippable + + + + + def _parse_step_to_action(self, generated_step, translated_step): + + loc = '' + + if self.use_translated: + parsed_step = translated_step.split(' ') + + if len(parsed_step) > 2: + [skill, obj, loc] = parsed_step + + else: + [skill, obj] = parsed_step + + else: + noun_start = [c for c in self.nlp(generated_step).noun_chunks][0].text + noun_substring = generated_step[generated_step.index(noun_start):] + + modified_skills_for_noun = [ + s + ' ' + noun_substring for s in self.reference_skills] + modified_skills_for_noun += self.robot_skills + + # embed the reference and the input prompt + embedded_ref = self.similarity_model.encode( + modified_skills_for_noun, convert_to_tensor=True) + embedded_prompt = self.similarity_model.encode( + generated_step, convert_to_tensor=True) + + # compute the cosine similarity + cosine_similarity = util.cos_sim(embedded_prompt, embedded_ref) + + skill_idx = torch.argmax(cosine_similarity).item() + + skill = (self.object_skills + self.robot_skills)[skill_idx] + obj = noun_substring + + + precondition_error, error_params, error_msg, skippable = self._check_precondition_error(skill, obj, loc, translated_step if self.use_translated else generated_step) + + + + return obj, skill, loc, precondition_error, error_params, error_msg, skippable + + + def _start_robot_command(self, desc, command_proto, end_time_secs=None): + + def _start_command(): + self.robot_command_client.robot_command(command=command_proto, + end_time_secs=end_time_secs) + + self._try_grpc(desc, _start_command) + + def _try_grpc(self, desc, thunk): + try: + return thunk() + except (ResponseError, RpcError) as err: + self.add_message("Failed {}: {}".format(desc, err)) + return None + + +class WalkToAction(): + + def __init__(self, hostname, action): + + self.action_interface = action + + if self.action_interface.with_robot: + self.recording_interface = RecordingInterface(self.action_interface.robot, self.action_interface.graph_path, self.action_interface.client_metadata) + + self.graphnav_interface = GraphNavInterface(self.action_interface.robot, self.recording_interface._download_filepath) + + + self.graphnav_interface._upload_graph_and_snapshots() + self.graphnav_interface._set_initial_localization_fiducial() + self.graphnav_interface._list_graph_waypoint_and_edge_ids() + + success = self.graphnav_interface._navigate_to(['origin']) + + print(success) + + + + + def navigate(self, text_prompt): + + + assert self.action_interface.with_robot is False or self.graphnav_interface is not None, 'Error in WalkToAction: need to define graphnav interface by generate_initial_scene_graph before navigate' + + + location, __ ,__, __, __, __, __ = self.action_interface._parse_step_to_action(None, text_prompt) + + success = False + position = None + + if self.action_interface.with_robot: + with LeaseKeepAlive(self.action_interface.lease_client, must_acquire=True, return_at_exit=True): + if location in self.action_interface.saved_waypoints: + print('walking to waypoint {} ...'.format(location)) + + if self.action_interface.with_pause: + out = continue_block() + + if out == 'n': + exit() + + success = self.graphnav_interface._navigate_to(location) + + else: + #directly apply get_pose_data_for_closest_image and then navigate to returned location + chosen_pose_data, _, chosen_idx = _get_pose_data_for_closest_image(text_prompt) + + print('walking to image at index {} ...'.format(chosen_idx)) + + if self.action_interface.with_pause: + out = continue_block() + + if out == 'n': + exit() + + position = chosen_pose_data['position']; rotation_matrix = chosen_pose_data['rotation_matrix'] + + success = self.graphnav_interface._navigate_to_anchor(position + rotation_matrix) + + #simple_move_to(self.robot, position, rotation_matrix) + #move_to(self.robot, self.robot_state_client, pose=position, distance_margin=1.00, hostname=self.hostname, end_time=30) + + #update robot location after navigation + if success or not self.action_interface.with_robot: + self.action_interface.object_states[location]['coordinates'] = position + self.action_interface.robot_location = location + self.action_interface.robot_misaligned = False + + return success or not self.action_interface.with_robot + + + def navigate_to_origin(self): + + success = False + + if self.action_interface.with_robot: + with LeaseKeepAlive(self.acction_interface.lease_client, must_acquire=True, return_at_exit=True): + success = self.graphnav_interface._navigate_to(location) + + if success or not self.action_interface.with_robot: + self.action_interface.robot_location = 'origin' + self.action_interface.robot_misaligned = False + return success or not self.action_interface.with_robot + + def collect_object_poses(self): + + if not os.path.isdir(self.action_interface.data_path): + os.mkdir(self.action_interface.data_path) + + # We only use the hand color + camera_sources = ['hand_depth_in_hand_color_frame', 'hand_color_image'] + + counter = 0 + img_to_pose_dir = {} # takes in counter as key, and returns robot pose. saved in img_dir + done = False + + all_embeddings = [] #stores a tensor of CLIP encoded images for all images taken in scene + + while True: + + if self.action_interface.manual_images: + response = input("Take image [y/n]") + if response == "n": + break + + else: + time.sleep(1/float(pic_hz)) + + # capture and save images to disk + image_responses = self.action_interface.image_client.get_image_from_sources( + camera_sources) + + # Image responses are in the same order as the requests. + if len(image_responses) < 2: + print('Error: failed to get images.') + return False + + frame_tree_snapshot = self.action_interface.robot.get_frame_tree_snapshot() + body_tform_hand = get_a_tform_b(frame_tree_snapshot, "body", "hand") + + graphnav_localization_state = self.graphnav_interface._graph_nav_client.get_localization_state() + seed_tform_body = graphnav_localization_state.localization.seed_tform_body + seed_tform_body = bosdyn.client.math_helpers.SE3Pose(seed_tform_body.position.x, seed_tform_body.position.y, seed_tform_body.position.z, seed_tform_body.rotation) + + seed_tform_hand = seed_tform_body * body_tform_hand + + img_to_pose_dir[counter] = {"position": [seed_tform_hand.position.x, seed_tform_hand.position.y, seed_tform_hand.position.z], + "quaternion(wxyz)": [seed_tform_hand.rotation.w, seed_tform_hand.rotation.x, seed_tform_hand.rotation.y, seed_tform_hand.rotation.z], + "rotation_matrix": seed_tform_hand.rotation.to_matrix(), + "rpy": [seed_tform_hand.rotation.to_roll(), seed_tform_hand.rotation.to_pitch(), seed_tform_hand.rotation.to_yaw()]} + + pickle.dump(img_to_pose_dir, open( + self.action_interface.data_path+self.action_interface.pose_data_fname, "wb")) + + robot_position = [seed_tform_hand.position.x, + seed_tform_hand.position.y, seed_tform_hand.position.z] + + # robot is moving around + if np.linalg.norm(np.array(robot_position) - np.array(last_robot_position)) > 0.2: + print("Robot is moving, no photos!") + last_robot_position = robot_position + already_took_photo = False + continue + else: + last_robot_position = [seed_tform_hand.position.x, + seed_tform_hand.position.y, seed_tform_hand.position.z] + + # cv_depth is in millimeters, divide by 1000 to get it into meters + cv_depth_meters = cv_depth / 1000.0 + + # Visual is a JPEG + cv_visual = cv2.imdecode(np.frombuffer( + image_responses[1].shot.image.data, dtype=np.uint8), -1) + + # Convert the visual image from a single channel to RGB so we can add color + visual_rgb = cv_visual if len(cv_visual.shape) == 3 else cv2.cvtColor( + cv_visual, cv2.COLOR_GRAY2RGB) + + # Map depth ranges to color + + # cv2.applyColorMap() only supports 8-bit; convert from 16-bit to 8-bit and do scaling + min_val = np.min(cv_depth) + max_val = np.max(cv_depth) + depth_range = max_val - min_val + depth8 = (255.0 / depth_range * + (cv_depth - min_val)).astype('uint8') + depth8_rgb = cv2.cvtColor(depth8, cv2.COLOR_GRAY2RGB) + depth_color = cv2.applyColorMap(depth8_rgb, cv2.COLORMAP_JET) + + # Add the two images together. + out = cv2.addWeighted(visual_rgb, 0.5, depth_color, 0.5, 0) + + cv2.imwrite(self.action_interface.data_path+"color_" + + str(counter)+".jpg", cv_visual) + pickle.dump(cv_depth_meters, open( + self.action_interface.data_path+"depth_"+str(counter), "wb")) + cv2.imwrite(self.action_interface.data_path + + "combined_"+str(counter)+".jpg", out) + + + #perform clip image embedding and save to file + device = torch.device('cuda:0') + + preprocessed_image = self.action_interface.clip_preprocess(cv_visual).unsqueeze(0).to(device) + + with torch.no_grad(): + clip_embedding = self.action_interface.clip_model.encode_image(preprocessed_image).cpu() + + all_embeddings.append(clip_embedding) + + counter += 1 + + if counter == 100: + done = True + + if done: + break + + #save the clip image embeddings from captured scene data + with open(self.action_interface.data_path+self.action_interface.embedding_data_fname,'wb') as f: + pickle.dump({'clip_embedding': torch.cat(all_embeddings, dim=0)}, f) + + print('%0d images saved' % counter) + + print('Moving back to origin ...') + success = self.graphnav_interface._navigate_to('origin') + self.action_interface.robot_location = 'origin' + self.action_interface.robot_misaligned = False + + return True and success + + def generate_initial_scene_graph(self): + + with LeaseKeepAlive(self.action_interface.lease_client, must_acquire=True, return_at_exit=True): + #clear out the scene graph + self.recording_interface._clear_map() + + while True: + print(""" + Options: + (0) Clear map. + (1) Start recording a map. + (2) Stop recording a map. + (3) Get the recording service's status. + (4) Create a default waypoint in the current robot's location. + (5) Create a named waypoint in the current robot's location + (6) Download the map after recording. + (7) List the waypoint ids and edge ids of the map on the robot. + (8) Create new edge between existing waypoints using odometry. + (9) Create new edge from last waypoint to first waypoint using odometry. + (10) Automatically find and close loops. + (a) Optimize the map's anchoring. + (q) Exit. + """) + try: + inputs = input('>') + except NameError: + pass + req_type = inputs.split(' ')[0] + + args = None + if len(inputs.split(' ')) > 1: + args = inputs.split(' ')[1:] + + if req_type == 'q': + print('Quitting data collection for scene graph') + break + + if req_type not in self.recording_interface._command_dictionary: + print("Request not in the known command dictionary.") + continue + + if req_type == '5' and args is None: + print('Add argument for the waypoint name after the command option') + continue + + try: + #add to saved waypoints if new waypoint generated + if req_type == '5': + self.action_interface.saved_waypoints.add(args[0]) + cmd_func = self.recording_interface._command_dictionary[req_type] + cmd_func(args) + except Exception as e: + print('Error: ', e) + continue + + #after collecting the map, download the entire graph + self.recording_interface._download_full_graph() + + #create graphnav interface and upload the saved graph after creating map + self.graphnav_interface = GraphNavInterface(self.robot, self.recording_interface._download_filepath) + self.graphnav_interface._upload_graph_and_snapshots() + + print('Moving back to origin ...') + success = self.graphnav_interface._navigate_to('origin') + self.action_interface.robot_location = 'origin' + self.action_interface.robot_misaligned = False + + return True and success + + + + def move_to_position_and_take_photo(self, topk_item, pose_dir): + # move the position and take the photo + # return cv file that can be stored as an image using cv2.imwrite + # reference nlpmal.py go_to_and_pick_top_k + best_pose = None + + img_name = topk_item[1][0] + file_num = int(img_name.split("_")[-1].split(".")[0]) + print("processing image {file_num}".format(file_num=file_num)) + depth_img = pickle.load( + open(self.img_dir_path+"/depth_"+str(file_num), "rb")) + rotation_matrix = pose_dir[file_num]['rotation_matrix'] + print(rotation_matrix) + position = pose_dir[file_num]['position'] + print(position) + + # ymin, xmin, ymax, xmax = topk_item[1][3:-1] + # print(ymin, xmin, ymax, xmax) + # center_y = int((ymin + ymax)/2.0) + # center_x = int((xmin + xmax)/2.0) + + # transformed_point, bad_point = pixel_to_vision_frame( + # center_y, center_x, depth_img, rotation_matrix, position) + # print("transformed point is {tp}".format( + # tp=transformed_point)) + # print("bad point is {bad_point}".format(bad_point)) + + simple_move_to(self.robot, position=position, + rotation_matrix=rotation_matrix) + + open_gripper(self.robot_command_client) + + # Capture and save images to disk + # reference get_depth_color_pose.py + image_responses = self.image_client.get_image_from_sources( + self.sources) + + if len(image_responses) < 2: + print('Error: failed to get images.') + return False + frame_tree_snapshot = self.robot.get_frame_tree_snapshot() + vision_tform_hand = get_a_tform_b( + frame_tree_snapshot, "vision", "hand") + loc_data = {"position": [vision_tform_hand.position.x, vision_tform_hand.position.y, vision_tform_hand.position.z], + "quaternion(wxyz)": [vision_tform_hand.rotation.w, vision_tform_hand.rotation.x, vision_tform_hand.rotation.y, vision_tform_hand.rotation.z], + "rotation_matrix": vision_tform_hand.rotation.to_matrix(), + "rpy": [vision_tform_hand.rotation.to_roll(), vision_tform_hand.rotation.to_pitch(), vision_tform_hand.rotation.to_yaw()]} + + + # Depth is a raw bytestream + cv_depth = np.frombuffer( + image_responses[0].shot.image.data, dtype=np.uint16) + cv_depth = cv_depth.reshape(image_responses[0].shot.image.rows, + image_responses[0].shot.image.cols) + + # cv_depth is in millimeters, divide by 1000 to get it into meters + cv_depth_meters = cv_depth / 1000.0 + + # Visual is a JPEG + cv_visual = cv2.imdecode(np.frombuffer( + image_responses[1].shot.image.data, dtype=np.uint8), -1) + + #cv2.imwrite("color.jpg", cv_visual) + + # Convert the visual image from a single channel to RGB so we can add color + visual_rgb = cv_visual if len(cv_visual.shape) == 3 else cv2.cvtColor( + cv_visual, cv2.COLOR_GRAY2RGB) + + # Map depth ranges to color + + # cv2.applyColorMap() only supports 8-bit; convert from 16-bit to 8-bit and do scaling + min_val = np.min(cv_depth) + max_val = np.max(cv_depth) + depth_range = max_val - min_val + depth8 = (255.0 / depth_range * + (cv_depth - min_val)).astype('uint8') + depth8_rgb = cv2.cvtColor(depth8, cv2.COLOR_GRAY2RGB) + depth_color = cv2.applyColorMap(depth8_rgb, cv2.COLORMAP_JET) + + # Add the two images together. + out = cv2.addWeighted(visual_rgb, 0.5, depth_color, 0.5, 0) + + return cv_visual, cv_depth_meters, out, loc_data + + def update_images(self, text_prompt, top_k): + # currently, given the text prompt, use the same procedure of grabbing to + # pick top k images and update them based on the locations. + # print("start generating original point cloud") + # self.original_pcd = make_pointcloud( + # data_path=self.data_path, pose_data_fname=self.pose_data_fname, pointcloud_fname=self.pointcloud_fname) + print("start update images") + pose_dir = pickle.load( + open(self.img_dir_path+"/"+self.pose_data_fname, 'rb')) + # TODO: add all jpg files under the path + img_names = [f for f in os.listdir( + "data/default_data") if f[:5] == 'color'] + print(img_names) + priority_queue_vild_dir, priority_queue_clip_dir, _ = get_best_clip_vild_dirs(model=self.clip_model, + preprocess=self.clip_preprocess, + img_names=img_names, + img_dir_path=self.img_dir_path, + cache_images=False, + cache_text=False, + img_dir_name="", + category_names=[ + text_prompt], + headless=True, + use_softmax=False) + updated_imgs = set() + print(priority_queue_vild_dir[text_prompt].qsize()) + while top_k > 0 and priority_queue_vild_dir[text_prompt].qsize() > 0: + vild_item = ( + priority_queue_vild_dir[text_prompt].get()) + # clip_item = ( + # priority_queue_clip_dir[text_prompt].get()) + + vild_img_name = vild_item[1][0] + # clip_img_name = clip_item[1][0] + print("should process " + vild_img_name) + print(vild_img_name not in updated_imgs) + + if vild_img_name not in updated_imgs: + updated_imgs.add(vild_img_name) + top_k -= 1 + file_num = int(vild_img_name.split("_")[-1].split(".")[0]) + # print(vild_img_name) + # rotation_matrix = pose_dir[file_num]['rotation_matrix'] + # position = pose_dir[file_num]['position'] + # print(rotation_matrix) + # print(position) + # re-take photos + cv_visual, cv_depth_meters, out, loc_data = self.move_to_position_and_take_photo( + topk_item=vild_item, pose_dir=pose_dir) + # overwrite the original photos and depth + # if err is not None: + print("delete original image " + vild_img_name) + os.remove(self.img_dir_path + "/" +vild_img_name) + print("write the updated image " + vild_img_name) + cv2.imwrite(self.img_dir_path + "/" + vild_img_name, cv_visual) + + os.remove(self.img_dir_path+"/depth_"+str(file_num)) + pickle.dump(cv_depth_meters, open( + self.img_dir_path+"/depth_"+str(file_num), "wb")) + + os.remove(self.img_dir_path+"/combined_" + + str(file_num)+".jpg") + cv2.imwrite(self.img_dir_path+"/combined_" + + str(file_num)+".jpg", out) + + # update pose_data + print("update pose data for file num " + str(file_num)) + pose_dir[file_num] = loc_data + + # if clip_img_name not in updated_imgs: + # updated_imgs.add(clip_img_name) + # # re-take photos + # cv_visual, cv_depth_meters, out = self.move_to_position_and_take_photo( + # topk_item=clip_item, pose_dir=pose_dir) + # # overwrite the original photos and depth + # cv2.imwrite(self.img_dir_path + "/" + clip_img_name, cv_visual) + # pickle.dump(cv_depth_meters, open( + # self.img_dir_path+"/depth_"+str(file_num), "wb")) + # cv2.imwrite(self.img_dir_path+"/combined_" + + # str(file_num)+".jpg", out) + + # remove old pose_data + pose_data_path = self.data_path + self.pose_data_fname + os.remove(pose_data_path) + # dump new pose data + pickle.dump(pose_dir, open( + pose_data_path, "wb")) + # regenerate pointcloud + print("start generating updated point cloud") + self.original_pcd = make_pointcloud( + data_path=self.data_path, pose_data_fname=self.pose_data_fname, pointcloud_fname="updated_pointcloud.pcd") + + # print("generate modified point cloud") + # self.original_pcd = make_pointcloud() + + print("Finished update images") + + +class GrabAction(): + + def __init__(self, hostname, action): + + + self.use_vild = True + self.use_bbox_center = False + self.action_interface = action + + def get_center_of_mask(self, thresholded_mask): + + + + if type(thresholded_mask) == np.ndarray: + thresholded_mask = torch.from_numpy(thresholded_mask) + + + mask_coord = torch.where(thresholded_mask > 0.0) + + if len(mask_coord[0])==0 or len(mask_coord[1])==0: + print('Error: could not generate mask!') + return None, None + + + + mean_y = torch.mean(mask_coord[0].float()) + mean_x = torch.mean(mask_coord[1].float()) + + + + distances = torch.sqrt(torch.square( + mask_coord[0]-mean_y) + torch.square(mask_coord[1]-mean_x)) + + + min_arg = torch.argmin(distances) + + + + center_y = mask_coord[0][min_arg] + center_x = mask_coord[1][min_arg] + return center_y, center_x + + + def put_in_basket(self): + + def make_robot_command(arm_joint_traj): + """ Helper function to create a RobotCommand from an ArmJointTrajectory. + The returned command will be a SynchronizedCommand with an ArmJointMoveCommand + filled out to follow the passed in trajectory. """ + + joint_move_command = arm_command_pb2.ArmJointMoveCommand.Request(trajectory=arm_joint_traj) + arm_command = arm_command_pb2.ArmCommand.Request(arm_joint_move_command=joint_move_command) + sync_arm = synchronized_command_pb2.SynchronizedCommand.Request(arm_command=arm_command) + arm_sync_robot_cmd = robot_command_pb2.RobotCommand(synchronized_command=sync_arm) + return RobotCommandBuilder.build_synchro_command(arm_sync_robot_cmd) + + sh0 = 0.0692 + sh1 = -1.5 + el0 = 1.652 + el1 = -0.0691 + wr0 = 1.622 + wr1 = 1.550 + + max_vel = wrappers_pb2.DoubleValue(value=1) + max_acc = wrappers_pb2.DoubleValue(value=5) + traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point( + sh0, sh1, el0, el1, wr0, wr1, time_since_reference_secs=10) + arm_joint_traj = arm_command_pb2.ArmJointTrajectory(points=[traj_point],maximum_velocity=max_vel, maximum_acceleration=max_acc) + # Make a RobotCommand + command = make_robot_command(arm_joint_traj) + + # Send the request + cmd_id = self.action_interface.robot_command_client.robot_command(command) + # self.robot.logger.info('Requesting a single point trajectory with unsatisfiable constraints.') + + # Query for feedback + feedback_resp = self.action_interface.robot_command_client.robot_command_feedback(cmd_id) + #self.robot.logger.info("Feedback for Example 2: planner modifies trajectory") + time_to_goal = duration_to_seconds(feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback.time_to_goal) + time.sleep(time_to_goal) + + + sh0 = 3.14 + sh1 = -1.3 + el0 = 1.3 + el1 = -0.0691 + wr0 = 1.622 + wr1 = 1.550 + + max_vel = wrappers_pb2.DoubleValue(value=1) + max_acc = wrappers_pb2.DoubleValue(value=5) + traj_point = RobotCommandBuilder.create_arm_joint_trajectory_point( + sh0, sh1, el0, el1, wr0, wr1, time_since_reference_secs=10) + arm_joint_traj = arm_command_pb2.ArmJointTrajectory(points=[traj_point],maximum_velocity=max_vel, maximum_acceleration=max_acc) + # Make a RobotCommand + command = make_robot_command(arm_joint_traj) + + # Send the request + cmd_id = self.action_interface.robot_command_client.robot_command(command) + #self.robot.logger.info('Requesting a single point trajectory with unsatisfiable constraints.') + + # Query for feedback + feedback_resp = self.action_interface.robot_command_client.robot_command_feedback(cmd_id) + #self.robot.logger.info("Feedback for Example 2: planner modifies trajectory") + time_to_goal = duration_to_seconds(feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_joint_move_feedback.time_to_goal) + time.sleep(time_to_goal) + + + time.sleep(0.75) + gripper_open = RobotCommandBuilder.claw_gripper_open_fraction_command(1.0) + cmd_id = self.action_interface.robot_command_client.robot_command(gripper_open) + time.sleep(10.0) + + + + def nlmap_grab(self, text_prompt, testing_df=None): + print('EXECUTING GRAB ACTION') + time1_start = time.time() + + obj, skill, loc, __, __, __ = self.action_interface._parse_step_to_action(None, text_prompt) + text_prompt = obj + + executed = False + + if self.action_interface.with_robot: + + with LeaseKeepAlive(self.action_interface.lease_client, must_acquire=True, return_at_exit=True): + + fig, axs = plt.subplots( + 1, 3, gridspec_kw={'width_ratios': [2, 1, 2]}) + + + # convert the location from the moving base frame to the world frame. + robot_state = self.action_interface.robot_state_client.get_robot_state() + odom_T_flat_body = get_a_tform_b( + robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) + + # look at a point 1 meters in front and 0.1 meters below. + # We are not specifying a hand location, the robot will pick one. + gaze_target_in_odom = odom_T_flat_body.transform_point( + x=+1.0, y=0.0, z=+0.05) + + gaze_command = RobotCommandBuilder.arm_gaze_command(gaze_target_in_odom[0], + gaze_target_in_odom[1], + gaze_target_in_odom[2], ODOM_FRAME_NAME) + + gaze_command_id = self.action_interface.robot_command_client.robot_command( + gaze_command) + + block_until_arm_arrives( + self.action_interface.robot_command_client, gaze_command_id, 4.0) + + # open the SPOT gripper once spot has raised its arm + changed its gaze + open_gripper(self.action_interface.robot_command_client) + + # Capture and save images to disk + image_responses = self.action_interface.image_client.get_image_from_sources( + self.action_interface.sources) + + cv_visual = cv2.imdecode(np.frombuffer( + image_responses[1].shot.image.data, dtype=np.uint8), -1) + + # print('DEPTH DATA TEST: ', image_responses[0].shot.image.data) + + if not os.path.isdir('./tmp'): + os.mkdir('./tmp') + + # NOTE: maybe replace with direct image feed so that we don't have to save the image + if not os.path.isdir('./saved_nlmap'): + os.mkdir('./saved_nlmap') + if not os.path.isdir('./saved_nlmap/raw_images'): + os.mkdir('./saved_nlmap/raw_images') + if not os.path.isdir('./saved_nlmap/segmented_images'): + os.mkdir('./saved_nlmap/segmented_images') + if not os.path.isdir('./saved_nlmap/generated_masks'): + os.mkdir('./saved_nlmap/generated_masks') + + num = len(os.listdir('./saved_nlmap/raw_images')) + 1 + cv2.imwrite( + "./saved_nlmap/raw_images/raw_image{}.jpg".format(num), cv_visual) + + cv2.imwrite("./tmp/color_curview.jpg", cv_visual) + + # NOTE: left cache_path as default argument for now + _, _, priority_queue_combined = get_best_clip_vild_dirs(self.action_interface.clip_model, self.action_interface.clip_preprocess, [ + "color_curview.jpg"], "./tmp", cache_images=False, cache_text=False, img_dir_name="", category_names=[text_prompt], headless=True, use_softmax=False) + + top_k_items = priority_queue_combined[text_prompt].get() + + (_, (img_name, _, _, ymin, xmin, ymax, xmax, + segmentations, max_score)) = top_k_items + + np.save('./saved_nlmap/generated_masks/mask{}'.format(num), + segmentations) + + load_to_df['generated_mask_file'] = 'mask{}'.format(num) + + if self.use_bbox_center: + ymin, xmin, ymax, xmax = top_k_items[1][3:7] + + center_y = int((ymin + ymax)/2.0) + center_x = int((xmin + xmax)/2.0) + + else: + generated_mask = top_k_items[1][-2] + + center_y, center_x = self.get_center_of_mask(generated_mask) + + best_pixel = (center_x, center_y) + + pixel = plt.Circle((center_x, center_y), 0.9, color='r') + + print('Best Pixel: ', (center_y, center_x)) + + axs[0].imshow(cv_visual) + axs[0].add_patch(pixel) + axs[0].add_patch(pixel) + axs[1].imshow(top_k_items[1][2]) + axs[2].imshow(top_k_items[1][-2]) + + plt.title(text_prompt) + + plt.savefig( + './saved_nlmap/segmented_images/segmented_image{}.png'.format(num)) + + load_to_df['color_image_file'] = "raw_image{}.jpg".format(num) + + time1_end = time.time() + + if self.action_interface.show_time: + print("Total Time: ", (time1_end-time1_start), " seconds") + + load_to_df['compute_time'] = time1_end - time1_start + + detected = input("Detected Object [0/1]: ") + detected = int(detected) + + load_to_df['detected'] = detected + + if detected == 0: + for f in os.listdir('./tmp'): + os.remove(os.path.join('./tmp', f)) + + testing_df = testing_df.append(load_to_df, ignore_index=True) + testing_df.to_csv('./grasping_test.csv', index=False) + exit() + else: + testing_df = testing_df.append(load_to_df, ignore_index=True) + testing_df.to_csv('./grasping_test.csv', index=False) + + execute = input("Execute grasp") + + success = grasp_object( + self.action_interface.robot_state_client, self.action_interface.manipulation_api_client, best_pixel, image_responses[1]) + + # if object grasped successfully, carry object + if success: + carry_cmd = RobotCommandBuilder.arm_carry_command() + self.action_interface.robot_command_client.robot_command(carry_cmd) + time.sleep(0.75) + # TODO: figure out basket stuff later + # self.put_in_basket() + else: + # Recovery after grasping + open_gripper(self.action_interface.robot_command_client) + + executed = success + + human_review = input("Executed[0/1]: ") + executed = int(executed or bool(human_review)) + + load_to_df['executed'] = executed + + print("Stowing Arm Away...") + stow = RobotCommandBuilder.arm_stow_command() + stow_command_id = self.action_interface.robot_command_client.robot_command(stow) + block_until_arm_arrives( + self.action_interface.robot_command_client, stow_command_id, self.action_interface.default_timeout) + print("Arm Stowed Away!") + + move_back_proto = RobotCommandBuilder.synchro_velocity_command( + v_x=-1.0, v_y=0.0, v_rot=0.0) + + self.action_interface._start_robot_command( + 'move_backward', move_back_proto, end_time_secs=time.time() + 2.0) + + print(os.listdir("./tmp")) + # clear out the temporary files stored from ViLD + for f in os.listdir('./tmp'): + os.remove(os.path.join('./tmp', f)) + + # replace last entry with the new load_to_df dict + testing_df.drop(testing_df.tail(1).index, inplace=True) + testing_df = testing_df.append(load_to_df, ignore_index=True) + testing_df.to_csv('./grasping_test.csv', index=False) + + if executed or not self.action_interface.with_robot: + self.action_interface.robot_holding = True + self.action_interface.robot_holding_object = obj + # self.action_interface.robot_location = None + self.action_interface.robot_misaligned = True + self.action_interface.object_states[obj]['held'] = True + self.action_interface.object_states[obj]['location'] = 'agent' + + if 'contained_in' in self.action_interface.object_states[obj] and self.action_interface.object_states[obj]['contained_in'] in self.action_interface.id_to_name: + self.action_interface.object_states[self.action_interface.id_to_name[self.action_interface.object_states[obj]['contained_in']]]['contains'].remove(self.action_interface.object_states[obj]['id']) + + self.action_interface.object_states[obj]['contained_in'] = None + + return executed or not self.action_interface.with_robot + + def clipseg_grab(self, text_prompt, testing_df=None): + + print('Executing grab action for {}'.format(text_prompt)) + time1_start = time.time() + + if self.action_interface.with_robot: + # with LeaseKeepAlive(self.lease_client, must_acquire=True, return_at_exit=True): + + fig, axs = plt.subplots(1, 3, gridspec_kw={'width_ratios': [2, 1, 2]}) + + # convert the location from the moving base frame to the world frame. + robot_state = self.action_interface.robot_state_client.get_robot_state() + odom_T_flat_body = get_a_tform_b( + robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) + + # look at a point 1 meters in front and 0.1 meters below. + # We are not specifying a hand location, the robot will pick one. + gaze_target_in_odom = odom_T_flat_body.transform_point( + x=+1.0, y=0.0, z=+0.05) + + gaze_command = RobotCommandBuilder.arm_gaze_command(gaze_target_in_odom[0], + gaze_target_in_odom[1], + gaze_target_in_odom[2], ODOM_FRAME_NAME) + + gaze_command_id = self.action_interface.robot_command_client.robot_command( + gaze_command) + + block_until_arm_arrives( + self.action_interface.robot_command_client, gaze_command_id, 4.0) + + # open the SPOT gripper once spot has raised its arm + changed its gaze + open_gripper(self.action_interface.robot_command_client) + + # Capture and save images to disk + image_responses = self.action_interface.image_client.get_image_from_sources( + self.action_interface.sources) + + time.sleep(10.0) + + + cv_visual = cv2.imdecode(np.frombuffer( + image_responses[1].shot.image.data, dtype=np.uint8), -1) + + cv_depth = np.frombuffer(image_responses[0].shot.image.data, dtype=np.uint16) + cv_depth = cv_depth.reshape(image_responses[0].shot.image.rows, image_responses[0].shot.image.cols) + + time.sleep(5.0) + + print('Running ClipSeg model...') + # make prediction on image + generated_mask, binary_mask = self.action_interface.clipseg_model.segment_image(cv_visual, cv_depth, text_prompt) + + + + if not os.path.isdir('./saved_clipseg'): + os.mkdir('./saved_clipseg') + if not os.path.isdir('./saved_clipseg/raw_images'): + os.mkdir('./saved_clipseg/raw_images') + if not os.path.isdir('./saved_clipseg/segmented_images'): + os.mkdir('./saved_clipseg/segmented_images') + + if not os.path.isdir('./saved_clipseg/binary_masks'): + os.mkdir('./saved_clipseg/binary_masks') + if not os.path.isdir('./saved_clipseg/clipseg_masks'): + os.mkdir('./saved_clipseg/clipseg_masks') + + + num = len(os.listdir('./saved_clipseg/raw_images')) + 1 + + cv2.imwrite( + "./saved_clipseg/raw_images/raw_image{}.jpg".format(num), cv_visual) + + + + cv2.imwrite("./tmp/color_curview.jpg", cv_visual) + + + if self.action_interface.use_clipseg_binary: + center_y, center_x = self.get_center_of_mask(binary_mask) + else: + center_index = torch.argmax(generated_mask).item() + center_y = center_index//generated_mask.shape[0] + center_x = center_index%(generated_mask.shape[0]) + + + np.save('./saved_clipseg/binary_masks/mask{}'.format(num), + binary_mask) + np.save('./saved_clipseg/clipseg_masks/mask{}'.format(num), + generated_mask) + # load_to_df['clipseg_mask_file'] = 'mask{}'.format(num) + # load_to_df['binary_mask_file'] = 'mask{}'.format(num) + + + best_pixel = (center_x, center_y) + + if center_x is not None and center_y is not None: + pixel = plt.Circle((center_x, center_y), 0.9, color='r') + + print('Best Pixel: ', (center_y, center_x)) + + axs[0].imshow(cv_visual) + if center_x is not None and center_y is not None: + axs[0].add_patch(pixel) + axs[1].imshow(generated_mask) + if center_x is not None or center_y is not None: + axs[2].imshow(binary_mask) + + plt.title(text_prompt) + + plt.savefig('./saved_clipseg/segmented_images/segmented_image{}.png'.format(num)) + + # load_to_df['color_image_file'] = "raw_image{}.jpg".format(num) + + time1_end = time.time() + + if self.action_interface.show_time: + print("Total Time: ", (time1_end-time1_start), " seconds") + + # load_to_df['compute_time'] = time1_end - time1_start + + detected = input("Detected Object [0/1]: ") + detected = int(detected) + + # load_to_df['detected'] = detected + + if detected == 0: + print('Did not detect correct target object') + for f in os.listdir('./tmp'): + os.remove(os.path.join('./tmp', f)) + + # testing_df = testing_df.append(load_to_df, ignore_index=True) + # testing_df.to_csv('./grasping_test.csv', index=False) + exit() + # else: + + # testing_df = testing_df.append(load_to_df, ignore_index=True) + # testing_df.to_csv('./grasping_test.csv', index=False) + + if self.action_interface.with_pause: + out = continue_block() + + if out == 'n': + exit() + + print('Grasping target object ...') + success = grasp_object( + self.action_interface.robot_state_client, self.action_interface.manipulation_api_client, best_pixel, image_responses[1]) + + # if object grasped successfully, carry object + if success: + carry_cmd = RobotCommandBuilder.arm_carry_command() + self.action_interface.robot_command_client.robot_command(carry_cmd) + time.sleep(0.75) + # self.put_in_basket() + else: + # Recovery after grasping + open_gripper(self.action_interface.robot_command_client) + + + human_review = input("Executed[0/1]: ") + executed = int(human_review) + + # load_to_df['executed'] = executed + + print("Stowing Arm Away...") + stow = RobotCommandBuilder.arm_stow_command() + stow_command_id = self.action_interface.robot_command_client.robot_command(stow) + block_until_arm_arrives( + self.action_interface.robot_command_client, stow_command_id, self.action_interface.default_timeout) + + + #move_back_proto = RobotCommandBuilder.synchro_velocity_command( + # v_x=-1.0, v_y=0.0, v_rot=0.0) + + #self.action_interface._start_robot_command( + # 'move_backward', move_back_proto, end_time_secs=time.time() + 2.0) + + + # clear out the temporary files stored from ViLD + for f in os.listdir('./tmp'): + os.remove(os.path.join('./tmp', f)) + + # replace last entry with the new load_to_df dict + # testing_df.drop(testing_df.tail(1).index, inplace=True) + # testing_df = testing_df.append(load_to_df, ignore_index=True) + # testing_df.to_csv('./grasping_test.csv', index=False) + + executed = False + if executed or not self.action_interface.with_robot: + self.action_interface.robot_holding = True + self.action_interface.robot_holding_object = obj + # self.action_interface.robot_location = None + self.action_interface.robot_misaligned = True + self.action_interface.object_states[obj]['held'] = True + self.action_interface.object_states[obj]['location'] = 'agent' + + if 'contained_in' in self.action_interface.object_states[obj] and self.action_interface.object_states[obj]['contained_in'] in self.action_interface.id_to_name: + self.action_interface.object_states[self.action_interface.id_to_name[self.action_interface.object_states[obj]['contained_in']]]['contains'].remove(self.action_interface.object_states[obj]['id']) + + self.action_interface.object_states[obj]['contained_in'] = None + + return executed or not self.action_interface.with_robot + +class StandSitAction(): + + def __init__(self, hostname, action): + self.action_interface = action + + + def stand_up(self, text_prompt=None): + + try: + + if self.action_interface.with_robot: + + # with LeaseKeepAlive(self.action_interface.lease_client, must_acquire=False, return_at_exit=True): + + blocking_stand(self.action_interface.robot_command_client, timeout_sec=self.action_interface.default_timeout) + self.action_interface.robot_standing = True + except: + return False + + return True + + def sit_down(self, text_prompt=None): + + try: + if self.action_interface.with_robot: + # with LeaseKeepAlive(self.action_interface.lease_client, must_acquire=False, return_at_exit=True): + + self.action_interface.robot_command_client.robot_command(command=RobotCommandBuilder.synchro_sit_command(), end_time_secs=self.action_interface.default_timeout) + self.action_interface.robot_standing = False + except: + return False + + return True + + +class OpenCloseAction(): + + def __init__(self, hostname, action): + self.action_interface = action + + + def get_center_of_mask(self, thresholded_mask): + + + + if type(thresholded_mask) == np.ndarray: + thresholded_mask = torch.from_numpy(thresholded_mask) + + + mask_coord = torch.where(thresholded_mask > 0.0) + + if len(mask_coord[0])==0 or len(mask_coord[1])==0: + print('Error: could not generate mask!') + return None, None + + + + mean_y = torch.mean(mask_coord[0].float()) + mean_x = torch.mean(mask_coord[1].float()) + + + + distances = torch.sqrt(torch.square( + mask_coord[0]-mean_y) + torch.square(mask_coord[1]-mean_x)) + + + min_arg = torch.argmin(distances) + + + + center_y = mask_coord[0][min_arg] + center_x = mask_coord[1][min_arg] + return center_y, center_x + + + + def open(self, text_prompt): + + with LeaseKeepAlive(self.action_interface.lease_client, must_acquire=True, return_at_exit=True): + + obj, skill, _, __, __,__, __ = self.action_interface._parse_step_to_action(None, text_prompt) + executed = False + + if obj == 'fridge': + obj = 'blue handle' + elif obj == 'door': + obj = 'door handle' + + fig, axs = plt.subplots(1, 3, gridspec_kw={'width_ratios': [2, 1, 2]}) + + robot_state = self.action_interface.robot_state_client.get_robot_state() + odom_T_flat_body = get_a_tform_b( + robot_state.kinematic_state.transforms_snapshot, ODOM_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME) + + # look at a point 1 meters in front and 0.1 meters below. + # We are not specifying a hand location, the robot will pick one. + gaze_target_in_odom = odom_T_flat_body.transform_point( + x=+1.0, y=0.0, z=+0.05) + + gaze_command = RobotCommandBuilder.arm_gaze_command(gaze_target_in_odom[0], + gaze_target_in_odom[1], + gaze_target_in_odom[2], ODOM_FRAME_NAME) + + gaze_command_id = self.action_interface.robot_command_client.robot_command( + gaze_command) + + block_until_arm_arrives( + self.action_interface.robot_command_client, gaze_command_id, 4.0) + + # open the SPOT gripper once spot has raised its arm + changed its gaze + open_gripper(self.action_interface.robot_command_client) + + # Capture and save images to disk + image_responses = self.action_interface.image_client.get_image_from_sources( + self.action_interface.sources) + + time.sleep(10.0) + + + cv_visual = cv2.imdecode(np.frombuffer( + image_responses[1].shot.image.data, dtype=np.uint8), -1) + + cv_depth = np.frombuffer(image_responses[0].shot.image.data, dtype=np.uint16) + cv_depth = cv_depth.reshape(image_responses[0].shot.image.rows, image_responses[0].shot.image.cols) + + time.sleep(5.0) + + print('Running ClipSeg model...') + # make prediction on image + generated_mask, binary_mask = self.action_interface.clipseg_model.segment_image(cv_visual, cv_depth, obj) + + if not os.path.isdir('./saved_clipseg/segmented_images_open'): + os.mkdir('./saved_clipseg/segmented_images_open') + + + cv2.imwrite("./tmp/color_curview.jpg", cv_visual) + + center_y, center_x = self.get_center_of_mask(binary_mask) + + + best_pixel = (center_x, center_y) + + if center_x is not None and center_y is not None: + pixel = plt.Circle((center_x, center_y), 0.9, color='r') + + print('Best Pixel: ', (center_y, center_x)) + + axs[0].imshow(cv_visual) + if center_x is not None and center_y is not None: + axs[0].add_patch(pixel) + axs[1].imshow(generated_mask) + if center_x is not None or center_y is not None: + axs[2].imshow(binary_mask) + + plt.title(text_prompt) + + plt.savefig('./saved_clipseg/segmented_images/segmented_image{}.png'.format(num)) + + detected = input("Detected Object [0/1]: ") + detected = int(detected) + + if detected == 0: + print('Did not detect correct target object') + exit() + + if self.action_interface.with_pause: + out = continue_block() + + if out == 'n': + exit() + + print('Grasping target object ...') + success = grasp_object(self.action_interface.robot_state_client, self.action_interface.manipulation_api_client, best_pixel, image_responses[1]) + + if success: + construct_drawer_task(velocity_normalized = -0.5, force_limit=80) + + if executed or not self.action_interface.with_robot: + # self.action_interface.robot_location = None + self.action_interface.object_states[obj]['open'] = True + self.action_interface.robot_misaligned = True + + return executed or not self.action_interface.with_robot + + + + def close(self, text_prompt): + obj, skill, _, __, __,__, __ = self.action_interface._parse_step_to_action(None, text_prompt) + executed = False + + # construct_drawer_task() + + if executed or not self.action_interface.with_robot: + # self.action_interface.robot_location = None + self.action_interface.object_states[obj]['open'] = True + self.action_interface.robot_misaligned = True + + return executed or not self.action_interface.with_robot + + +class PutDownAction(): + + def __init__(self, hostname, action): + self.action_interface = action + + def put_down(self, text_prompt=None): + + # obj, skill, loc, __, __,__, __ = self.action_interface._parse_step_to_action(None, text_prompt) + + executed = True + + if self.action_interface.with_robot: + + # with LeaseKeepAlive(self.action_interface.lease_client, must_acquire=True, return_at_exit=True): + + try: + x1 = 0.75 # a reasonable position in front of the robot + x2 = 1.0 + x3 = 1.2 + y1 = 0 # centered + z1 = 0.2 # at the body's height + z2 = 0.0 + + # Use the same rotation as the robot's body. + rotation = math_helpers.Quat() + rotation_roll = math_helpers.Quat.from_roll(3.14/2.0) + print(f"rotation roll {rotation_roll}") + print(f"euler version: {math_helpers.quat_to_eulerZYX(rotation_roll)}") + print(f"original roll {rotation}") + print(f"euler version: {math_helpers.quat_to_eulerZYX(rotation)}") + + # Define times (in seconds) for each point in the trajectory. + t_first_point = 0 # first point starts at t = 0 for the trajectory. + t_second_point = 5.0 + t_third_point = 7.0 + + # Build the points in the trajectory. + hand_pose1 = math_helpers.SE3Pose(x=x1, y=y1, z=z1, rot=rotation) + hand_pose2 = math_helpers.SE3Pose(x=x2, y=y1, z=z1, rot=rotation) + hand_pose3 = math_helpers.SE3Pose(x=x3, y=y1, z=z2, rot=rotation_roll) + + # Build the points by combining the pose and times into protos. + traj_point1 = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose1.to_proto(), time_since_reference=seconds_to_duration(t_first_point)) + traj_point2 = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose2.to_proto(), time_since_reference=seconds_to_duration(t_second_point)) + traj_point3 = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose3.to_proto(), time_since_reference=seconds_to_duration(t_third_point)) + + # Build the trajectory proto by combining the points. + hand_traj = trajectory_pb2.SE3Trajectory(points=[traj_point1, traj_point2, traj_point3]) + + # Build the command by taking the trajectory and specifying the frame it is expressed + # in. + # + # In this case, we want to specify the trajectory in the body's frame, so we set the + # root frame name to the flat body frame. + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + pose_trajectory_in_task=hand_traj, root_frame_name=GRAV_ALIGNED_BODY_FRAME_NAME) + + # Pack everything up in protos. + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command) + + synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command) + + robot_command = robot_command_pb2.RobotCommand(synchronized_command=synchronized_command) + + # Keep the gripper closed the whole time. + robot_command = RobotCommandBuilder.claw_gripper_open_fraction_command(0.0, build_on_command=robot_command) + + x1 = 0.75 # a reasonable position in front of the robot + x2 = 1.0 + x3 = 1.2 + y1 = 0 # centered + z = 0.2 # at the body's height + + # Use the same rotation as the robot's + + # Send the trajectory to the robot. + cmd_id = self.action_interface.robot_command_client.robot_command(robot_command) + + # Wait until the arm arrives at the goal. + while True: + feedback_resp = self.action_interface.robot_command_client.robot_command_feedback(cmd_id) + self.action_interface.robot.logger.info('Distance to final point: ' + '{:.2f} meters'.format( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback. + arm_cartesian_feedback.measured_pos_distance_to_goal) + ', {:.2f} radians'.format( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback. + arm_cartesian_feedback.measured_rot_distance_to_goal)) + + if feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE: + self.action_interface.robot.logger.info('Move complete.') + break + time.sleep(0.1) + + ###### ROBOT OPENS GRIPPER + traj_point4 = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose3.to_proto(), time_since_reference=seconds_to_duration(0)) + traj_point5 = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose3.to_proto(), time_since_reference=seconds_to_duration(1.0)) + traj_point6 = trajectory_pb2.SE3TrajectoryPoint( + pose=hand_pose1.to_proto(), time_since_reference=seconds_to_duration(5.0)) + + # Build the trajectory proto by combining the points. + hand_traj = trajectory_pb2.SE3Trajectory(points=[traj_point4, traj_point5, traj_point6]) + + + # In this case, we want to specify the trajectory in the body's frame, so we set the + # root frame name to the flat body frame. + arm_cartesian_command = arm_command_pb2.ArmCartesianCommand.Request( + pose_trajectory_in_task=hand_traj, root_frame_name=GRAV_ALIGNED_BODY_FRAME_NAME) + + # Pack everything up in protos. + arm_command = arm_command_pb2.ArmCommand.Request( + arm_cartesian_command=arm_cartesian_command) + + synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( + arm_command=arm_command) + + robot_command = robot_command_pb2.RobotCommand(synchronized_command=synchronized_command) + + # Keep the gripper open the whole time. + robot_command = RobotCommandBuilder.claw_gripper_open_fraction_command( + 1.0, build_on_command=robot_command) + + # Send the trajectory to the robot. + cmd_id = self.action_interface.robot_command_client.robot_command(robot_command) + + # Wait until the arm arrives at the goal. + while True: + feedback_resp = self.action_interface.robot_command_client.robot_command_feedback(cmd_id) + self.action_interface.robot.logger.info('Distance to final point: ' + '{:.2f} meters'.format( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback. + arm_cartesian_feedback.measured_pos_distance_to_goal) + ', {:.2f} radians'.format( + feedback_resp.feedback.synchronized_feedback.arm_command_feedback. + arm_cartesian_feedback.measured_rot_distance_to_goal)) + + if feedback_resp.feedback.synchronized_feedback.arm_command_feedback.arm_cartesian_feedback.status == arm_command_pb2.ArmCartesianCommand.Feedback.STATUS_TRAJECTORY_COMPLETE: + self.action_interface.robot.logger.info('Move complete.') + break + time.sleep(0.1) + + ### STOW ARM + stow = RobotCommandBuilder.arm_stow_command() + stow_command_id = self.action_interface.robot_command_client.robot_command(stow) + block_until_arm_arrives(self.action_interface.robot_command_client, stow_command_id,10.0) + + except: + executed = False + + executed = False + if executed or not self.action_interface.with_robot: + self.action_interface.robot_holding = False + self.action_interface.robot_holding_object = None + self.action_interface.robot_location = loc + self.action_interface.robot_misaligned = True + self.action_interface.object_states[obj]['held'] = False + self.action_interface.object_states[obj]['location'] = loc + + if 'contained_in' in self.action_interface.object_states[obj]: + self.action_interface.object_states[obj]['contained_in'] = self.action_interface.object_states[loc]['id'] + + + self.action_interface.object_states[loc]['contains'].append(self.action_interface.object_states[obj]['id']) + + + + return executed or not self.action_interface.with_robot + +# if __name__ == '__main__': + +# global testing_df +# if not os.path.isfile('./grasping_test.csv'): + +# testing_df = pd.DataFrame(columns=['phrase', 'trial', 'ablation', 'grounded_verb', 'cosine_sim', 'parsed_noun', +# 'detected', 'mask_iou', 'clipseg_mask_file', 'binary_mask_file', 'color_image_file', 'executed', 'compute_time']) +# else: +# testing_df = pd.read_csv('./grasping_test.csv') + +# load_to_df = {c: None for c in ['phrase', 'trial', 'ablation', 'grounded_verb', 'cosine_sim', 'parsed_noun', +# 'detected', 'mask_iou', 'clipseg_mask_file', 'binary_mask_file', 'color_image_file', 'executed', 'compute_time']} + +# grab_action_executor = GrabAction("tusker") + + +# TEXT_PROMPT = input('Text Prompt: ') + +# skill_idx, cosine_sim, parsed_noun = grab_action_executor.find_closest_action( +# TEXT_PROMPT) + +# load_to_df['cosine_sim'] = cosine_sim +# load_to_df['parsed_noun'] = parsed_noun + +# print('Parsed Noun: ', parsed_noun) +# print('Grounded Verb: ', grab_action_executor.reference_skills[skill_idx]) + +# trial = int(input('Trial Number: ')) +# ABLATION = input('Ablation Type: ') + +# load_to_df['phrase'] = TEXT_PROMPT +# load_to_df['trial'] = trial +# load_to_df['ablation'] = ABLATION + +# load_to_df['grounded_verb'] = grab_action_executor.reference_skills[skill_idx] +# load_to_df['parsed_noun'] = parsed_noun + +# grab_action_executor.clipseg_grab(parsed_noun, testing_df) + + + + + +if __name__ == '__main__': + + # import pdb + # pdb.set_trace() + # action = Action('138.16.161.22', None, with_robot=True) + + + # walk_executor = WalkToAction('138.16.161.22', action) + # standsit_executor = StandSitAction('138.16.161.22', action) + # grab_executor = GrabAction('138.16.161.22', action) + # putdown_executor = PutDownAction('138.16.161.22', action) + + # standsit_executor.sit_down() + # standsit_executor.stand_up() + # walk_executor.graphnav_interface._navigate_to(['shoe_rack']) + # grab_executor.clipseg_grab('shoes') + # walk_executor.graphnav_interface._navigate_to(['door']) + # putdown_executor.put_down() + # walk_executor.graphnav_interface._navigate_to(['office_table']) + # grab_executor.clipseg_grab('black cap') + # walk_executor.graphnav_interface._navigate_to(['office_table']) + # putdown_executor.put_down() + # walk_executor.graphnav_interface._navigate_to(['office_table']) + # grab_executor.clipseg_grab('energy drink') + # walk_executor.graphnav_interface._navigate_to(['door']) + # putdown_executor.put_down() + # standsit_executor.sit_down() + pass + + + \ No newline at end of file diff --git a/spot/collect_spot.py b/spot/collect_spot.py new file mode 100644 index 00000000..74c29c08 --- /dev/null +++ b/spot/collect_spot.py @@ -0,0 +1,175 @@ +import bosdyn.client +import json +import time +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from get_image import capture + + +IP = '138.16.161.24' +USER = 'user' +PASS = "bigbubbabigbubba" + +previous_gripper_open_percentage = None + +def is_moving(robot_state, threshold=0.05): + """Determine if the robot is moving in any direction or rotating.""" + # velocity = robot_state.kinematic_state.velocity.vel + linear_velocity = robot_state.kinematic_state.velocity_of_body_in_vision.linear + angular_velocity = robot_state.kinematic_state.velocity_of_body_in_vision.angular + + return (abs(linear_velocity.x) > threshold or + abs(linear_velocity.y) > threshold or + abs(linear_velocity.z) > threshold or + abs(angular_velocity.x) > threshold or + abs(angular_velocity.y) > threshold or + abs(angular_velocity.z) > threshold) + +def is_arm_moving(manipulator_state, linear_threshold=0.1, angular_threshold=0.1): + """Determine if the robot's arm is moving.""" + # Choose either 'velocity_of_hand_in_vision' or 'velocity_of_hand_in_odom' based on your requirement + linear_velocity = manipulator_state.velocity_of_hand_in_vision.linear + angular_velocity = manipulator_state.velocity_of_hand_in_vision.angular + # print(angular_velocity) + # print(linear_velocity) + + # Check if the linear or angular velocity exceeds the thresholds + linear_moving = abs(linear_velocity.x) > linear_threshold or abs(linear_velocity.y) > linear_threshold or abs(linear_velocity.z) > linear_threshold + angular_moving = abs(angular_velocity.x) > angular_threshold or abs(angular_velocity.y) > angular_threshold or abs(angular_velocity.z) > angular_threshold + + return linear_moving or angular_moving + + +def is_gripper_moving(manipulator_state, threshold=0.01): + global previous_gripper_open_percentage + current_percentage = manipulator_state.gripper_open_percentage + + if previous_gripper_open_percentage is None: + previous_gripper_open_percentage = current_percentage + return False + + if abs(current_percentage - previous_gripper_open_percentage) > threshold: + previous_gripper_open_percentage = current_percentage + return True + + previous_gripper_open_percentage = current_percentage + return False + + + +def collect_data(robot, robot_state): + """Collect and return required data.""" + front_image_data = capture(robot, "PIXEL_FORMAT_RGB_U8") + front_depth_data = capture(robot, "PIXEL_FORMAT_DEPTH_U16") + front_depth_color_data = capture(robot) + gripper_image_data = capture(robot, mode="arm") + + body_state_odom = robot_state.kinematic_state.velocity_of_body_in_odom + body_state_vision = robot_state.kinematic_state.velocity_of_body_in_vision + arm_state_odom = robot_state.manipulator_state.velocity_of_hand_in_odom + arm_state_vision = robot_state.manipulator_state.velocity_of_hand_in_vision + gripper_open_percent = robot_state.manipulator_state.gripper_open_percentage + stow_state = robot_state.manipulator_state.stow_state + + return { + "images": { + # "front_rgb_camera": list(front_image_data), + # "front_depth_camera": list(front_depth_data), + # "front_depth_color_camera": list(front_depth_color_data), + # "gripper_camera": list(gripper_image_data) + }, + "body_state_odom": { + "linear": { + "x": body_state_odom.linear.x, + "y": body_state_odom.linear.y, + "z": body_state_odom.linear.z + }, + "angular": { + "x": body_state_odom.angular.x, + "y": body_state_odom.angular.y, + "z": body_state_odom.angular.z + } + }, + "body_state_vision": { + "linear": { + "x": body_state_vision.linear.x, + "y": body_state_vision.linear.y, + "z": body_state_vision.linear.z + }, + "angular": { + "x": body_state_vision.angular.x, + "y": body_state_vision.angular.y, + "z": body_state_vision.angular.z + } + }, + "arm_state_odom": { + "linear": { + "x": arm_state_odom.linear.x, + "y": arm_state_odom.linear.y, + "z": arm_state_odom.linear.z + }, + "angular": { + "x": arm_state_odom.angular.x, + "y": arm_state_odom.angular.y, + "z": arm_state_odom.angular.z + } + }, + "arm_state_vision": { + "linear": { + "x": arm_state_vision.linear.x, + "y": arm_state_vision.linear.y, + "z": arm_state_vision.linear.z + }, + "angular": { + "x": arm_state_vision.angular.x, + "y": arm_state_vision.angular.y, + "z": arm_state_vision.angular.z + } + }, + "gripper_open_percent": gripper_open_percent, + "stow_state": stow_state + } + +def is_robot_sitting(robot_state): + """Determine if the robot is in a sitting position.""" + # Placeholder for demonstration + # Replace with actual logic to determine if the robot is sitting + vision_position = robot_state.kinematic_state.transforms_snapshot.child_to_parent_edge_map['vision'].parent_tform_child.position + if vision_position.z >= 0.15: + return True + return False + + +def main(): + # Create robot object and authenticate + sdk = bosdyn.client.create_standard_sdk('SpotRobotClient') + robot = sdk.create_robot(IP) + robot.authenticate(USER, PASS) + + + # Create state, image, and manipulation clients + state_client = robot.ensure_client(RobotStateClient.default_service_name) + + data_sequence = [] + + while True: + robot_state = state_client.get_robot_state() + # collected_data = collect_data(robot, robot_state) + + if is_robot_sitting(robot_state): + with open('spot_data2.json', 'w') as file: + json.dump(data_sequence, file, indent=4) + print('Data saved!') + break + + if is_moving(robot_state) or is_arm_moving(robot_state.manipulator_state) or is_gripper_moving(robot_state.manipulator_state): + print('moving') + collected_data = collect_data(robot, robot_state) + data_sequence.append(collected_data) + else: + print('not moving') + + time.sleep(0.1) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/spot/collect_spot_data.py b/spot/collect_spot_data.py new file mode 100644 index 00000000..8544b487 --- /dev/null +++ b/spot/collect_spot_data.py @@ -0,0 +1,896 @@ +#TODO LIST +''' +5) Double check data for the arm position in global frame (IDK) +8) Double check frame of reference for joint positions +''' + +#DONE LIST +''' +6) Folder structure renaming: data_{x}/folder_{y}/.... (all the images/datainside) + data_chunk_{y}.json + image_{y}.npy +7) Save json file using zipfile directly for floatingpoint values +4) Find ways to quit the generated thread deployed for data collection (IDK) +2) Best way of representing depth images (EASY) +3) Saving images as npy files instead of image directly (EASY) +9) Collect joint angles too if needed +1) Color images from left and right front fish eye lenses (MED) +''' + +"""Command line interface for graph nav with options to download/upload a map and to navigate a map. """ + +import argparse +import logging +import math +import os +from io import BytesIO +import sys +from datetime import datetime +import time +import pickle +import google.protobuf.timestamp_pb2 +import graph_nav_util +import grpc +import pdb +import cv2 +import numpy as np +import bosdyn.client.channel +import bosdyn.client.util +from bosdyn.api import geometry_pb2, power_pb2, robot_state_pb2 +from bosdyn.api.graph_nav import graph_nav_pb2, map_pb2, nav_pb2 +from bosdyn.client.exceptions import ResponseError +from bosdyn.client.frame_helpers import get_odom_tform_body +from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.lease import LeaseClient, LeaseKeepAlive, ResourceAlreadyClaimedError +from bosdyn.client.math_helpers import Quat, SE3Pose +from bosdyn.client.power import PowerClient, power_on, safe_power_off +from bosdyn.client.robot_command import RobotCommandBuilder, RobotCommandClient +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client.image import ImageClient, build_image_request +from bosdyn.client.frame_helpers import VISION_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME, get_a_tform_b, GRAV_ALIGNED_BODY_FRAME_NAME, ODOM_FRAME_NAME +from bosdyn.api.image_pb2 import Image +import threading +import selectors +import json +import zipfile + + +LOCK = threading.Lock() + +class GraphNavInterface(object): + """GraphNav service command line interface.""" + + def __init__(self, robot, upload_path, task_name): + self._robot = robot + + # Force trigger timesync. + self._robot.time_sync.wait_for_sync() + + # Create robot state and command clients. + self._robot_command_client = self._robot.ensure_client( + RobotCommandClient.default_service_name) + self._robot_state_client = self._robot.ensure_client(RobotStateClient.default_service_name) + + self.image_client = self._robot.ensure_client(ImageClient.default_service_name) + + + + self.previous_robot_state = None + self.previous_gripper_percentage = None + self.previous_robot_position = [0,0,0] + self.previous_robot_orientation = [None, None, None] + + # Create the client for the Graph Nav main service. + self._graph_nav_client = self._robot.ensure_client(GraphNavClient.default_service_name) + + # Create a power client for the robot. + self._power_client = self._robot.ensure_client(PowerClient.default_service_name) + + # Boolean indicating the robot's power state. + power_state = self._robot_state_client.get_robot_state().power_state + self._started_powered_on = (power_state.motor_power_state == power_state.STATE_ON) + self._powered_on = self._started_powered_on + + + + # Store the most recent knowledge of the state of the robot based on rpc calls. + self._current_graph = None + self._current_edges = dict() #maps to_waypoint to list(from_waypoint) + self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot + self._current_edge_snapshots = dict() # maps id to edge snapshot + self._current_annotation_name_to_wp_id = dict() + + # Filepath for uploading a saved graph's and snapshots too. + if upload_path[-1] == "/": + self._upload_filepath = upload_path[:-1] + else: + self._upload_filepath = upload_path + + self._command_dictionary = { + '1': self._get_localization_state, + '2': self._set_initial_localization_fiducial, + '3': self._set_initial_localization_waypoint, + '4': self._list_graph_waypoint_and_edge_ids, + '5': self._upload_graph_and_snapshots, + '8': self._navigate_to_anchor, + '9': self._clear_graph, + '0': self._toggle_data_collection + } + + #additional data for toggling continuous data collection + self.collect_data = False + self.pic_hz = 3 + + #store the current task name in natural language + self.task_name = task_name + self.scene_name = upload_path.split('/')[-1] + + curr_index = len(os.listdir('./Trajectories/trajectories')) + + self.save_base_path = os.path.join('./Trajectories/trajectories/data_{}'.format(curr_index)) + if not os.path.exists(self.save_base_path): + os.mkdir(self.save_base_path) + self.data_counter = 0 + + def _get_localization_state(self, *args): + """Get the current localization and state of the robot.""" + state = self._graph_nav_client.get_localization_state() + print('Got localization: \n%s' % str(state.localization)) + odom_tform_body = get_odom_tform_body(state.robot_kinematics.transforms_snapshot) + print('Got robot state in kinematic odometry frame: \n%s' % str(odom_tform_body)) + + def _set_initial_localization_fiducial(self, *args): + """Trigger localization when near a fiducial.""" + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot).to_proto() + # Create an empty instance for initial localization since we are asking it to localize + # based on the nearest fiducial. + localization = nav_pb2.Localization() + self._graph_nav_client.set_localization(initial_guess_localization=localization, + ko_tform_body=current_odom_tform_body) + + + def _set_initial_localization_waypoint(self, args): + """Trigger localization to a waypoint.""" + # Take the first argument as the localization waypoint. + if len(args) < 1: + # If no waypoint id is given as input, then return without initializing. + print("No waypoint specified to initialize to.") + return + destination_waypoint = graph_nav_util.find_unique_waypoint_id( + args[0], self._current_graph, self._current_annotation_name_to_wp_id) + if not destination_waypoint: + # Failed to find the unique waypoint id. + return + + robot_state = self._robot_state_client.get_robot_state() + current_odom_tform_body = get_odom_tform_body( + robot_state.kinematic_state.transforms_snapshot).to_proto() + # Create an initial localization to the specified waypoint as the identity. + localization = nav_pb2.Localization() + localization.waypoint_id = destination_waypoint + localization.waypoint_tform_body.rotation.w = 1.0 + self._graph_nav_client.set_localization( + initial_guess_localization=localization, + # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). + max_distance=0.2, + max_yaw=20.0 * math.pi / 180.0, + fiducial_init=graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NO_FIDUCIAL, + ko_tform_body=current_odom_tform_body) + + def _list_graph_waypoint_and_edge_ids(self, *args): + """List the waypoint ids and edge ids of the graph currently on the robot.""" + + # Download current graph + graph = self._graph_nav_client.download_graph() + if graph is None: + print("Empty graph.") + return + self._current_graph = graph + + localization_id = self._graph_nav_client.get_localization_state().localization.waypoint_id + + # Update and print waypoints and edges + self._current_annotation_name_to_wp_id, self._current_edges = graph_nav_util.update_waypoints_and_edges( + graph, localization_id) + + def _upload_graph_and_snapshots(self, *args): + """Upload the graph and snapshots to the robot.""" + print("Loading the graph from disk into local storage...") + with open(self._upload_filepath + "/graph", "rb") as graph_file: + # Load the graph from disk. + data = graph_file.read() + self._current_graph = map_pb2.Graph() + self._current_graph.ParseFromString(data) + print("Loaded graph has {} waypoints and {} edges".format( + len(self._current_graph.waypoints), len(self._current_graph.edges))) + for waypoint in self._current_graph.waypoints: + # Load the waypoint snapshots from disk. + with open(self._upload_filepath + "/waypoint_snapshots/{}".format(waypoint.snapshot_id), + "rb") as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + self._current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot + for edge in self._current_graph.edges: + if len(edge.snapshot_id) == 0: + continue + # Load the edge snapshots from disk. + with open(self._upload_filepath + "/edge_snapshots/{}".format(edge.snapshot_id), + "rb") as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot + # Upload the graph to the robot. + print("Uploading the graph and snapshots to the robot...") + true_if_empty = not len(self._current_graph.anchoring.anchors) + response = self._graph_nav_client.upload_graph(graph=self._current_graph, + generate_new_anchoring=true_if_empty) + # Upload the snapshots to the robot. + for snapshot_id in response.unknown_waypoint_snapshot_ids: + waypoint_snapshot = self._current_waypoint_snapshots[snapshot_id] + self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot) + print("Uploaded {}".format(waypoint_snapshot.id)) + for snapshot_id in response.unknown_edge_snapshot_ids: + edge_snapshot = self._current_edge_snapshots[snapshot_id] + self._graph_nav_client.upload_edge_snapshot(edge_snapshot) + print("Uploaded {}".format(edge_snapshot.id)) + + # The upload is complete! Check that the robot is localized to the graph, + # and if it is not, prompt the user to localize the robot before attempting + # any navigation commands. + localization_state = self._graph_nav_client.get_localization_state() + if not localization_state.localization.waypoint_id: + # The robot is not localized to the newly uploaded graph. + print("\n") + print("Upload complete! The robot is currently not localized to the map; please localize", \ + "the robot using commands (2) or (3) before attempting a navigation command.") + + def _navigate_to_anchor(self, *args): + """Navigate to a pose in seed frame, using anchors.""" + # The following options are accepted for arguments: [x, y], [x, y, yaw], [x, y, z, yaw], + # [x, y, z, qw, qx, qy, qz]. + # When a value for z is not specified, we use the current z height. + # When only yaw is specified, the quaternion is constructed from the yaw. + # When yaw is not specified, an identity quaternion is used. + + if len(args) < 1 or len(args[0]) not in [2, 3, 4, 7]: + print("Invalid arguments supplied.") + return + + seed_T_goal = SE3Pose(float(args[0][0]), float(args[0][1]), 0.0, Quat()) + + if len(args[0]) in [4, 7]: + seed_T_goal.z = float(args[0][2]) + else: + localization_state = self._graph_nav_client.get_localization_state() + if not localization_state.localization.waypoint_id: + print("Robot not localized") + return + seed_T_goal.z = localization_state.localization.seed_tform_body.position.z + + if len(args[0]) == 3: + seed_T_goal.rot = Quat.from_yaw(float(args[0][2])) + elif len(args[0]) == 4: + seed_T_goal.rot = Quat.from_yaw(float(args[0][3])) + elif len(args[0]) == 7: + seed_T_goal.rot = Quat(w=float(args[0][3]), x=float(args[0][4]), y=float(args[0][5]), + z=float(args[0][6])) + + if not self.toggle_power(should_power_on=True): + print("Failed to power on the robot, and cannot complete navigate to request.") + return False + + nav_to_cmd_id = None + # Navigate to the destination. + is_finished = False + while not is_finished: + # Issue the navigation command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + try: + nav_to_cmd_id = self._graph_nav_client.navigate_to_anchor( + seed_T_goal.to_proto(), 1.0, command_id=nav_to_cmd_id) + except ResponseError as e: + print("Error while navigating {}".format(e)) + return False + time.sleep(.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the navigation command is complete. Then sit + # the robot down once it is finished. + is_finished = self._check_success(nav_to_cmd_id) + + return is_finished + + def _navigate_to(self, args): + """Navigate to a specific waypoint.""" + # Take the first argument as the destination waypoint. + if len(args) < 1: + # If no waypoint id is given as input, then return without requesting navigation. + print("No waypoint provided as a destination for navigate to.") + return False + + destination_waypoint = graph_nav_util.find_unique_waypoint_id( + args[0], self._current_graph, self._current_annotation_name_to_wp_id) + if not destination_waypoint: + # Failed to find the appropriate unique waypoint id for the navigation command. + return False + + nav_to_cmd_id = None + # Navigate to the destination waypoint. + is_finished = False + while not is_finished: + # Issue the navigation command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + try: + nav_to_cmd_id = self._graph_nav_client.navigate_to(destination_waypoint, 1.0, + command_id=nav_to_cmd_id) + except ResponseError as e: + print("Error while navigating {}".format(e)) + break + time.sleep(.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the navigation command is complete. Then sit + # the robot down once it is finished. + is_finished = self._check_success(nav_to_cmd_id) + + return is_finished + + def _navigate_route(self, *args): + """Navigate through a specific route of waypoints.""" + if len(args) < 1 or len(args[0]) < 1: + # If no waypoint ids are given as input, then return without requesting navigation. + print("No waypoints provided for navigate route.") + return + waypoint_ids = args[0] + for i in range(len(waypoint_ids)): + waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id( + waypoint_ids[i], self._current_graph, self._current_annotation_name_to_wp_id) + if not waypoint_ids[i]: + # Failed to find the unique waypoint id. + return + + edge_ids_list = [] + all_edges_found = True + # Attempt to find edges in the current graph that match the ordered waypoint pairs. + # These are necessary to create a valid route. + for i in range(len(waypoint_ids) - 1): + start_wp = waypoint_ids[i] + end_wp = waypoint_ids[i + 1] + edge_id = self._match_edge(self._current_edges, start_wp, end_wp) + if edge_id is not None: + edge_ids_list.append(edge_id) + else: + all_edges_found = False + print("Failed to find an edge between waypoints: ", start_wp, " and ", end_wp) + print( + "List the graph's waypoints and edges to ensure pairs of waypoints has an edge." + ) + break + + if all_edges_found: + if not self.toggle_power(should_power_on=True): + print("Failed to power on the robot, and cannot complete navigate route request.") + return + + # Navigate a specific route. + route = self._graph_nav_client.build_route(waypoint_ids, edge_ids_list) + is_finished = False + while not is_finished: + # Issue the route command about twice a second such that it is easy to terminate the + # navigation command (with estop or killing the program). + nav_route_command_id = self._graph_nav_client.navigate_route( + route, cmd_duration=1.0) + time.sleep(.5) # Sleep for half a second to allow for command execution. + # Poll the robot for feedback to determine if the route is complete. Then sit + # the robot down once it is finished. + is_finished = self._check_success(nav_route_command_id) + + # Power off the robot if appropriate. + if self._powered_on and not self._started_powered_on: + # Sit the robot down + power off after the navigation command is complete. + self.toggle_power(should_power_on=False) + + def _clear_graph(self, *args): + """Clear the state of the map on the robot, removing all waypoints and edges.""" + return self._graph_nav_client.clear_graph() + + def _toggle_data_collection(self, *args): + """Toggle the data collection boolean""" + self.collect_data = True if self.collect_data is False else False + + def toggle_power(self, should_power_on): + """Power the robot on/off dependent on the current power state.""" + is_powered_on = self.check_is_powered_on() + if not is_powered_on and should_power_on: + # Power on the robot up before navigating when it is in a powered-off state. + power_on(self._power_client) + motors_on = False + while not motors_on: + future = self._robot_state_client.get_robot_state_async() + state_response = future.result( + timeout=10) # 10 second timeout for waiting for the state response. + if state_response.power_state.motor_power_state == robot_state_pb2.PowerState.STATE_ON: + motors_on = True + else: + # Motors are not yet fully powered on. + time.sleep(.25) + elif is_powered_on and not should_power_on: + # Safe power off (robot will sit then power down) when it is in a + # powered-on state. + safe_power_off(self._robot_command_client, self._robot_state_client) + else: + # Return the current power state without change. + return is_powered_on + # Update the locally stored power state. + self.check_is_powered_on() + return self._powered_on + + def check_is_powered_on(self): + """Determine if the robot is powered on or off.""" + power_state = self._robot_state_client.get_robot_state().power_state + self._powered_on = (power_state.motor_power_state == power_state.STATE_ON) + return self._powered_on + + def _check_success(self, command_id=-1): + """Use a navigation command id to get feedback from the robot and sit when command succeeds.""" + if command_id == -1: + # No command, so we have no status to check. + return False + status = self._graph_nav_client.navigation_feedback(command_id) + if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL: + # Successfully completed the navigation commands! + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST: + print("Robot got lost when navigating the route, the robot will now sit down.") + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK: + print("Robot got stuck when navigating the route, the robot will now sit down.") + return True + elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED: + print("Robot is impaired.") + return True + else: + # Navigation command is not complete yet. + return False + + def _match_edge(self, current_edges, waypoint1, waypoint2): + """Find an edge in the graph that is between two waypoint ids.""" + # Return the correct edge id as soon as it's found. + for edge_to_id in current_edges: + for edge_from_id in current_edges[edge_to_id]: + if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id(from_waypoint=waypoint2, to_waypoint=waypoint1) + elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id): + # This edge matches the pair of waypoints! Add it the edge list and continue. + return map_pb2.Edge.Id(from_waypoint=waypoint1, to_waypoint=waypoint2) + return None + + def _on_quit(self): + """Cleanup on quit from the command line interface.""" + # Sit the robot down + power off after the navigation command is complete. + if self._powered_on and not self._started_powered_on: + self._robot_command_client.robot_command(RobotCommandBuilder.safe_power_off_command(), + end_time_secs=time.time()) + + def is_arm_moving(self, curr_robot_state, linear_threshold=0.1, angular_threshold=0.1): + """Determine if the robot's arm is moving.""" + # Choose either 'velocity_of_hand_in_vision' or 'velocity_of_hand_in_odom' based on your requirement + linear_velocity = curr_robot_state.manipulator_state.velocity_of_hand_in_vision.linear + angular_velocity = curr_robot_state.manipulator_state.velocity_of_hand_in_vision.angular + + + # Check if the linear or angular velocity exceeds the thresholds + linear_moving = abs(linear_velocity.x) > linear_threshold or abs(linear_velocity.y) > linear_threshold or abs(linear_velocity.z) > linear_threshold + angular_moving = abs(angular_velocity.x) > angular_threshold or abs(angular_velocity.y) > angular_threshold or abs(angular_velocity.z) > angular_threshold + + return linear_moving or angular_moving + + + def is_gripper_moving(self, curr_robot_state, threshold=0.01): + + current_gripper_percentage = curr_robot_state.manipulator_state.gripper_open_percentage + + output = False + + if self.previous_gripper_percentage is None or abs(current_gripper_percentage - self.previous_gripper_percentage) >= threshold: + + output = True + + return output + + def is_robot_sitting(self, robot_state): + """Determine if the robot is in a sitting position.""" + # Placeholder for demonstration + # Replace with actual logic to determine if the robot is sitting + vision_position = robot_state.kinematic_state.transforms_snapshot.child_to_parent_edge_map['vision'].parent_tform_child.position + if vision_position.z >= 0.15: + return True + return False + + def is_body_moving(self, robot_curr_position, robot_curr_orientation, distance_threshold=0.1, angle_threshold=0.1): + + moving = False; rotating = False + + if self.previous_robot_position is None or abs(robot_curr_position[0]-self.previous_robot_position[0])>=distance_threshold or abs(robot_curr_position[1]-self.previous_robot_position[1])>=distance_threshold or abs(robot_curr_position[2]-self.previous_robot_position[2])>=distance_threshold: + moving = True + if None in self.previous_robot_orientation or abs(robot_curr_orientation[0]-self.previous_robot_orientation[0])>=angle_threshold or abs(robot_curr_orientation[1]-self.previous_robot_orientation[1])>=angle_threshold or abs(robot_curr_orientation[2]-self.previous_robot_orientation[2])>=angle_threshold: + rotating = True + + return moving or rotating + + def collect_images(self): + depth_sources = ['hand_depth_in_hand_color_frame', 'frontleft_depth','frontright_depth'] + image_sources = ['hand_color_image', 'frontleft_fisheye_image', 'frontright_fisheye_image'] + #'hand_color_in_hand_depth_frame' + + + image_responses = [build_image_request(source, quality_percent=90, pixel_format=Image.PIXEL_FORMAT_RGB_U8) for source in image_sources] + image_responses = self.image_client.get_image(image_responses) + + + depth_responses = [build_image_request(source, quality_percent=90, pixel_format=Image.PIXEL_FORMAT_DEPTH_U16) for source in depth_sources] + depth_responses = self.image_client.get_image(depth_responses) + + + + #hand depth image: extract and convert from mm to meters (div by 1000) + hand_depth = np.frombuffer(depth_responses[0].shot.image.data, dtype=np.uint16) + hand_depth = hand_depth.reshape(depth_responses[0].shot.image.rows, depth_responses[0].shot.image.cols) + + #if the min and max depth on image are < 12cm (length of gripper) then object is there + object_held = int(np.min(hand_depth) <= 0.12 and np.max(hand_depth) <= 0.12) + + + #hand color image + hand_color_image = cv2.imdecode(np.frombuffer(image_responses[0].shot.image.data, dtype=np.uint8), -1) + hand_color_image = hand_color_image if len(hand_color_image.shape)==3 else cv2.cvtColor(hand_color_image, cv2.COLOR_GRAY2RGB) + + #left fisheye depth + left_fisheye_depth = np.frombuffer(depth_responses[1].shot.image.data, dtype=np.uint16) + left_fisheye_depth = left_fisheye_depth.reshape(depth_responses[1].shot.image.rows, depth_responses[1].shot.image.cols) + + #left fisheye image + left_fisheye_image = cv2.imdecode(np.frombuffer(image_responses[1].shot.image.data, dtype=np.uint8), -1) + left_fisheye_image = np.rot90(left_fisheye_image, k=3) + left_fisheye_image = left_fisheye_image if len(left_fisheye_image.shape)==3 else cv2.cvtColor(left_fisheye_image, cv2.COLOR_GRAY2RGB) + + #right fisheye depth + right_fisheye_depth = np.frombuffer(depth_responses[2].shot.image.data, dtype=np.uint16) + right_fisheye_depth = right_fisheye_depth.reshape(depth_responses[2].shot.image.rows, depth_responses[2].shot.image.cols) + + #right fisheye image + right_fisheye_image = cv2.imdecode(np.frombuffer(image_responses[2].shot.image.data, dtype=np.uint8),-1) + right_fisheye_image = np.rot90(right_fisheye_image, k=3) + right_fisheye_image = right_fisheye_image if len(right_fisheye_image.shape)==3 else cv2.cvtColor(right_fisheye_image, cv2.COLOR_GRAY2RGB) + + #hand color in depth + # hand_color_in_depth = cv2.imdecode(np.frombuffer(image_responses[3].shot.image.data, dtype=np.uint8), -1) + # hand_color_in_depth = hand_color_in_depth if len(hand_color_in_depth.shape)==3 else cv2.cvtColor(hand_color_in_depth, cv2.COLOR_GRAY2RGB) + + + return hand_depth, hand_color_image, left_fisheye_depth, left_fisheye_image, right_fisheye_depth, right_fisheye_image, object_held + + + def collect_images_and_metadata(self): + + curr_robot_state = self._robot_state_client.get_robot_state() + + frame_tree_snapshot = self._robot.get_frame_tree_snapshot() + body_tform_hand = get_a_tform_b(frame_tree_snapshot, "body", "hand") + odom_tform_body = get_a_tform_b(frame_tree_snapshot, "odom", "body") + + + #get all the graphnav related localizations, positions and orientations relative to the seed frame + graphnav_localization_state = self._graph_nav_client.get_localization_state() + + + seed_tform_body = graphnav_localization_state.localization.seed_tform_body + seed_tform_body = bosdyn.client.math_helpers.SE3Pose(seed_tform_body.position.x, seed_tform_body.position.y, seed_tform_body.position.z, seed_tform_body.rotation) + + + seed_tform_hand = seed_tform_body*body_tform_hand + + seed_tform_body_rotation_matrix = seed_tform_body.to_matrix()[:-1,:-1] + body_tform_odom_rotation_matrix = odom_tform_body.inverse().to_matrix()[:-1,:-1] + + #---------------------------------------------------------------------# + + robot_curr_position = np.array([seed_tform_body.position.x,seed_tform_body.position.y, seed_tform_body.position.z]) + robot_curr_quaternion = np.array([seed_tform_body.rotation.w, seed_tform_body.rotation.x, seed_tform_body.rotation.y, seed_tform_body.rotation.z]) + robot_curr_orientation = np.array([seed_tform_hand.rotation.to_roll(), seed_tform_hand.rotation.to_pitch(), seed_tform_hand.rotation.to_yaw()]) + + robot_curr_velocity_in_odom_frame = curr_robot_state.kinematic_state.velocity_of_body_in_odom + robot_linear_velocity_in_seed_frame = np.array([robot_curr_velocity_in_odom_frame.linear.x, robot_curr_velocity_in_odom_frame.linear.y, robot_curr_velocity_in_odom_frame.linear.z]) + robot_linear_velocity_in_seed_frame = np.matmul(seed_tform_body_rotation_matrix, np.matmul(body_tform_odom_rotation_matrix, robot_linear_velocity_in_seed_frame)) + + robot_angular_velocity_in_seed_frame = np.array([robot_curr_velocity_in_odom_frame.angular.x, robot_curr_velocity_in_odom_frame.angular.y, robot_curr_velocity_in_odom_frame.angular.z]) + robot_angular_velocity_in_seed_frame = np.matmul(seed_tform_body_rotation_matrix, np.matmul(body_tform_odom_rotation_matrix, robot_angular_velocity_in_seed_frame)) + + arm_curr_velocity_in_odom_frame = curr_robot_state.manipulator_state.velocity_of_hand_in_odom + arm_linear_velocity_in_seed_frame = np.array([arm_curr_velocity_in_odom_frame.linear.x, arm_curr_velocity_in_odom_frame.linear.y, arm_curr_velocity_in_odom_frame.linear.z]) + arm_linear_velocity_in_seed_frame = np.matmul(seed_tform_body_rotation_matrix, np.matmul(body_tform_odom_rotation_matrix, arm_linear_velocity_in_seed_frame)) + + arm_angular_velocity_in_seed_frame = np.array([arm_curr_velocity_in_odom_frame.angular.x, arm_curr_velocity_in_odom_frame.angular.y, arm_curr_velocity_in_odom_frame.angular.z]) + arm_angular_velocity_in_seed_frame = np.matmul(seed_tform_body_rotation_matrix, np.matmul(body_tform_odom_rotation_matrix, arm_angular_velocity_in_seed_frame)) + + + arm_curr_position_rel_body = np.array([body_tform_hand.position.x, body_tform_hand.position.y, body_tform_hand.position.z]) + arm_curr_quaternion_rel_body = np.array([body_tform_hand.rotation.w, body_tform_hand.rotation.x, body_tform_hand.rotation.y, body_tform_hand.rotation.z]) + arm_curr_orientation_rel_body = np.array([body_tform_hand.rotation.to_roll(), body_tform_hand.rotation.to_pitch(), body_tform_hand.rotation.to_yaw()]) + + arm_curr_position_rel_seed = np.array([seed_tform_hand.position.x, body_tform_hand.y, body_tform_hand.z]) + arm_curr_quaternion_rel_seed = np.array([seed_tform_hand.rotation.w, seed_tform_hand.rotation.x, seed_tform_hand.rotation.y, seed_tform_hand.rotation.z]) + arm_curr_orientation_rel_seed = np.array([seed_tform_hand.rotation.to_roll(), seed_tform_hand.rotation.to_pitch(), seed_tform_hand.rotation.to_yaw()]) + + #additional state variables e.g. stowing and grasping + current_gripper_percentage = curr_robot_state.manipulator_state.gripper_open_percentage + current_stow_state = int(curr_robot_state.manipulator_state.stow_state) + + #TODO: figure out how to make this in the body frame + joint_states = {x.name: x.position.value for x in curr_robot_state.kinematic_state.joint_states} + joint_velocities = {x.name: x.velocity.value for x in curr_robot_state.kinematic_state.joint_states} + curr_time = datetime.utcnow().strftime('%H:%M:%S.%f')[:-3] + + + #get the position of the foot relative to body and seed frames + feet_curr_position_rel_seed = [] + for idx in range(len(curr_robot_state.foot_state)): + foot_position = np.array([curr_robot_state.foot_state[idx].foot_position_rt_body.x, curr_robot_state.foot_state[idx].foot_position_rt_body.y, curr_robot_state.foot_state[idx].foot_position_rt_body.z]) + foot_position = np.matmul(seed_tform_body_rotation_matrix, foot_position) + feet_curr_position_rel_seed.append({'x': foot_position[0], 'y': foot_position[1], 'z': foot_position[2]}) + + feet_curr_position_rel_body = [] + for idx in range(len(curr_robot_state.foot_state)): + feet_curr_position_rel_body.append({'x':curr_robot_state.foot_state[idx].foot_position_rt_body.x,'y':curr_robot_state.foot_state[idx].foot_position_rt_body.y, 'z':curr_robot_state.foot_state[idx].foot_position_rt_body.z}) + + + #collect and save all the necessary images + store the image paths + hand_depth, hand_color_image, left_fisheye_depth, left_fisheye_image, right_fisheye_depth, right_fisheye_image, object_held = self.collect_images() + + curr_save_path = os.path.join(self.save_base_path, 'folder_{}.zip'.format(self.data_counter)) + # if not os.path.exists(curr_save_path): + # os.mkdir(curr_save_path) + + + + #Step 1: check robot is not sitting down and that it's gripper/arm/body are moving + with LOCK: + arm = self.is_arm_moving(curr_robot_state) + gripper = self.is_gripper_moving(curr_robot_state) + sitting = self.is_robot_sitting(curr_robot_state) + body = self.is_body_moving(robot_curr_position, robot_curr_orientation) + if arm or gripper or body: + # print('arm ', arm) + # print('gripper ', gripper) + # print('body ', body) + print('COLLECTING DATA') + # os.mkdir(curr_save_path) + #Step 2: if all conditions are met, capture the current data including + ''' + Language command (given) + Front RGB images + Front Depth images + Gripper RGB images + Wall clock time + Gripper RGB images + Body state (x, y, z, roll, pitch, yaw) + Arm state (x, y, z, roll, pitch, yaw) + Body quaternion + Arm quaternion + Arm stow state (True/False) + Gripper open percentage + Current held object(s) + Front instance segmentation + Gripper instance segmentation + ''' + + + # pickle.dump(hand_depth, open(os.path.join(curr_save_path, 'hand_depth_{}'.format(self.data_counter)), "wb")) + # pickle.dump(left_fisheye_depth, open(os.path.join(curr_save_path, 'left_fisheye_depth_{}'.format(self.data_counter)), "wb")) + # pickle.dump(right_fisheye_depth, open(os.path.join(curr_save_path, 'right_fisheye_depth'), "wb")) + # cv2.imwrite(os.path.join(curr_save_path, 'hand_depth.jpeg'), hand_depth) + # cv2.imwrite(os.path.join(curr_save_path, 'left_fisheye_depth.jpeg'), left_fisheye_depth) + # cv2.imwrite(os.path.join(curr_save_path, 'right_fisheye_depth.jpeg'), right_fisheye_depth) + # cv2.imwrite(os.path.join(curr_save_path, 'hand_color_image.jpeg'), hand_color_image) + # cv2.imwrite(os.path.join(curr_save_path, 'left_fisheye_image.jpeg'), left_fisheye_image) + # cv2.imwrite(os.path.join(curr_save_path, 'right_fisheye_image.jpeg'), right_fisheye_image) + + global current_data + current_data = {'language_command': self.task_name, + 'scene_name':self.scene_name, + 'wall_clock_time': curr_time, + 'left_fisheye_rgb': os.path.join(curr_save_path, 'left_fisheye_image_{}.npy'.format(self.data_counter)), + 'left_fisheye_depth': os.path.join(curr_save_path, 'left_fisheye_depth_{}.npy'.format(self.data_counter)), + 'right_fisheye_rgb': os.path.join(curr_save_path, 'right_fisheye_image_{}.npy'.format(self.data_counter)), + 'right_fisheye_depth': os.path.join(curr_save_path, 'right_fisheye_depth_{}.npy'.format(self.data_counter)), + 'gripper_rgb': os.path.join(curr_save_path, 'gripper_image_{}.npy'.format(self.data_counter)), + 'gripper_depth': os.path.join(curr_save_path, 'gripper_depth_{}.npy'.format(self.data_counter)), + 'left_fisheye_instance_seg': os.path.join(curr_save_path, 'left_fisheye_image_instance_seg_{}.npy'.format(self.data_counter)), + 'right_fisheye_instance_seg': os.path.join(curr_save_path, 'right_fisheye_image_instance_seg_{}.npy'.format(self.data_counter)), + 'gripper_fisheye_instance_seg': os.path.join(curr_save_path, 'gripper_image_instance_seg_{}.npy'.format(self.data_counter)), + 'body_state': {'x': robot_curr_position[0], 'y':robot_curr_position[1], 'z': robot_curr_position[2]}, + 'body_quaternion': {'w': robot_curr_quaternion[0], 'x': robot_curr_quaternion[1], 'y':robot_curr_quaternion[2], 'z':robot_curr_quaternion[3]}, + 'body_orientation': {'r': robot_curr_orientation[0], 'p': robot_curr_orientation[1], 'y': robot_curr_orientation[2]}, + 'body_linear_velocity': {'x':robot_linear_velocity_in_seed_frame[0], 'y':robot_linear_velocity_in_seed_frame[1], 'z': robot_linear_velocity_in_seed_frame[2]}, + 'body_angular_velocity': {'x': robot_angular_velocity_in_seed_frame[0], 'y': robot_angular_velocity_in_seed_frame[1], 'z': robot_angular_velocity_in_seed_frame[2]}, + 'arm_state_rel_body': {'x': arm_curr_position_rel_body[0], 'y': arm_curr_position_rel_body[1], 'z': arm_curr_position_rel_body[2]}, + 'arm_quaternion_rel_body': {'w': arm_curr_quaternion_rel_body[0], 'x': arm_curr_quaternion_rel_body[1], 'y': arm_curr_quaternion_rel_body[2], 'z': arm_curr_quaternion_rel_body[2]}, + 'arm_orientation_rel_body': {'x': arm_curr_orientation_rel_body[0], 'y': arm_curr_orientation_rel_body[1], 'z': arm_curr_orientation_rel_body[2]}, + 'arm_state_global': {'x': arm_curr_position_rel_seed[0], 'y': arm_curr_position_rel_seed[1], 'z': arm_curr_position_rel_seed[2]}, + 'arm_quaternion_global': {'w': arm_curr_quaternion_rel_seed[0], 'x':arm_curr_quaternion_rel_seed[1], 'y': arm_curr_quaternion_rel_seed[2], 'z':arm_curr_quaternion_rel_seed[3]}, + 'arm_orientation_global': {'x':arm_curr_orientation_rel_seed[0], 'y':arm_curr_orientation_rel_seed[1], 'z':arm_curr_orientation_rel_seed[2]}, + 'arm_linear_velocity': {'x':arm_linear_velocity_in_seed_frame[0], 'y':arm_linear_velocity_in_seed_frame[1], 'z':arm_linear_velocity_in_seed_frame[2]}, + 'arm_angular_velocity': {'x': arm_angular_velocity_in_seed_frame[0], 'y':arm_angular_velocity_in_seed_frame[1], 'z':arm_angular_velocity_in_seed_frame[2]}, + 'arm_stowed': current_stow_state, + 'gripper_open_percentage': current_gripper_percentage, + 'object_held': object_held, + 'feet_state_rel_body': feet_curr_position_rel_body, + 'feet_state_global': feet_curr_position_rel_seed, + 'all_joint_angles': joint_states, + 'all_joint_velocities': joint_velocities, + + + } + + with zipfile.ZipFile(curr_save_path, 'w', zipfile.ZIP_DEFLATED) as zipf: + + with zipf.open('data_chunk_{}.json'.format(self.data_counter), 'w') as json_file: + # Convert the chunk to a JSON string and encode it to bytes + json_data = json.dumps(current_data).encode('utf-8') + json_file.write(json_data) + + # Define a helper function to save numpy data + def save_npy_to_zip(data, filename): + npy_data = BytesIO() + np.save(npy_data, data, allow_pickle=True, fix_imports=True) + npy_data.seek(0) # Rewind the buffer + with zipf.open(filename, 'w') as npy_file: + npy_file.write(npy_data.getvalue()) + + + + + save_npy_to_zip(hand_depth, 'gripper_depth_{}.npy'.format(self.data_counter)) + save_npy_to_zip(left_fisheye_depth, 'left_fisheye_depth_{}.npy'.format(self.data_counter)) + save_npy_to_zip(right_fisheye_depth, 'right_fisheye_depth_{}.npy'.format(self.data_counter)) + save_npy_to_zip(hand_color_image, 'gripper_image_{}.npy'.format(self.data_counter)) + save_npy_to_zip(left_fisheye_image,'left_fisheye_image_{}.npy'.format(self.data_counter)) + save_npy_to_zip(right_fisheye_image, 'right_fisheye_image_{}.npy'.format(self.data_counter)) + + + self.data_counter += 1 + + + else: + print('NO DATA COLLECTED: ROBOT IS NOT MOVING') + + self.previous_gripper_percentage = current_gripper_percentage + self.previous_robot_state = curr_robot_state + self.previous_robot_position = robot_curr_position + self.previous_robot_orientation = robot_curr_orientation + + + def run_data_collection(self): + last_collect = datetime.now() + while self.collect_data: + if (datetime.now() - last_collect).total_seconds() >= 1 / float(self.pic_hz): + + print((datetime.now() - last_collect).total_seconds()) + + last_collect = datetime.now() + self.collect_images_and_metadata() + + #if (end - start).total_seconds() < 1/float(self.pic_hz): + # # 3Hz frequency for data collection: capture sample every 1/3 seconds + # time.sleep(1/float(self.pic_hz) - (end - start).total_seconds()) + + def run(self): + """Main loop for the command line interface.""" + + self._upload_graph_and_snapshots() + + #localize graphnav with initial fiducial and list out waypoints with edges + self._set_initial_localization_fiducial() + self._list_graph_waypoint_and_edge_ids() + + data_collection_thread = threading.Thread(target=self.run_data_collection, name='data_collection') + data_collection_thread.daemon = True + self.collection_thread_spawned = False + + while True: + print(""" + Options: + (1) Get localization state. + (2) Initialize localization to the nearest fiducial (must be in sight of a fiducial). + (3) Initialize localization to a specific waypoint (must be exactly at the waypoint. + (4) List the waypoint ids and edge ids of the map on the robot. + (5) Upload the graph and its snapshots. + (6) Navigate to. The destination waypoint id is the second argument. + (7) Navigate route. The (in-order) waypoint ids of the route are the arguments. + (8) Navigate to in seed frame. The following options are accepted for arguments: [x, y], + [x, y, yaw], [x, y, z, yaw], [x, y, z, qw, qx, qy, qz]. (Don't type the braces). + When a value for z is not specified, we use the current z height. + When only yaw is specified, the quaternion is constructed from the yaw. + When yaw is not specified, an identity quaternion is used. + (9) Clear the current graph. + (0) Toggle data collection mode. + (q) Exit. + """) + selector = selectors.DefaultSelector() + selector.register(sys.stdin, selectors.EVENT_READ) + + try: + events = selector.select(timeout=None) + for key, mask in events: + if key.fileobj == sys.stdin: + inputs = sys.stdin.readline().strip() + except NameError: + pass + req_type = str.split(inputs)[0] + + if req_type == 'q': + self._on_quit() + break + + + if req_type not in self._command_dictionary: + print("Request not in the known command dictionary.") + continue + try: + cmd_func = self._command_dictionary[req_type] + cmd_func(str.split(inputs)[1:]) + except Exception as e: + print(e) + + + + if self.collect_data is True and self.collection_thread_spawned is False: + data_collection_thread.start() + # self.collect_images_and_metadata() + self.collection_thread_spawned = True + print('Started collection thread!') + if self.collect_data is False and self.collection_thread_spawned is True: + data_collection_thread.join() + self.collection_thread_spawned = False + print('Terminated collection thread!') + # time.sleep(1/float(self.pic_hz)) + + + + + +def main(argv): + + # HOSTNAME = '138.16.160.202' + # HOSTNAME = 'gouger' + HOSTNAME = '138.16.161.23' + USER = 'user' + PASS = 'bigbubbabigbubba' + + """Run the command-line interface.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('-u', '--upload-filepath', + help='Full filepath to graph and snapshots to be uploaded.', required=True) + parser.add_argument('-t', '--task', help='Task name in language', required=True) + #bosdyn.client.util.add_base_arguments(parser) + options = parser.parse_args(argv) + + # Setup and authenticate the robot. + sdk = bosdyn.client.create_standard_sdk('GraphNavClient') + robot = sdk.create_robot(HOSTNAME) + robot.authenticate(USER, PASS) + + graph_nav_command_line = GraphNavInterface(robot, options.upload_filepath, options.task) + + try: + graph_nav_command_line.run() + return True + except Exception as exc: # pylint: disable=broad-except + print(exc) + print("Graph nav command line client threw an error.") + return False + + + +if __name__ == '__main__': + exit_code = 0 + if not main(sys.argv[1:]): + exit_code = 1 + os._exit(exit_code) # Exit hard, no cleanup that could block. diff --git a/spot/get_image.py b/spot/get_image.py new file mode 100644 index 00000000..2d6a1ac6 --- /dev/null +++ b/spot/get_image.py @@ -0,0 +1,145 @@ +# Copyright (c) 2023 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +#This BD code is heavily modified by Ahmed Jaafar + +import cv2 +import numpy as np +from bosdyn.api import image_pb2 +from bosdyn.client.image import ImageClient, build_image_request + +ROTATION_ANGLE = { + 'back_fisheye_image': 0, + 'frontleft_fisheye_image': -78, + 'frontright_fisheye_image': -102, + 'left_fisheye_image': 0, + 'right_fisheye_image': 180 +} + + +def pixel_format_type_strings(): + names = image_pb2.Image.PixelFormat.keys() + return names[1:] + + +def pixel_format_string_to_enum(enum_string): + return dict(image_pb2.Image.PixelFormat.items()).get(enum_string) + + +def transform(image, dtype, num_bytes): + img = np.frombuffer(image.shot.image.data, dtype=dtype) + if image.shot.image.format == image_pb2.Image.FORMAT_RAW: + try: + # Attempt to reshape array into a RGB rows X cols shape. + img = img.reshape((image.shot.image.rows, image.shot.image.cols, num_bytes)) + except ValueError: + # Unable to reshape the image data, trying a regular decode. + img = cv2.imdecode(img, -1) + else: + img = cv2.imdecode(img, -1) + + return img + + +def capture(robot, format=None, mode="front"): + robot.sync_with_directory() + robot.time_sync.wait_for_sync() + + image_client = robot.ensure_client(ImageClient.default_service_name) + img_sources = ['frontleft_fisheye_image','frontright_fisheye_image'] + depth_sources = ['frontright_depth', 'frontleft_depth'] + imgs = [] + if mode == "front": + #rgb + if format == "PIXEL_FORMAT_RGB_U8": + + pixel_format = pixel_format_string_to_enum(format) + + image_request = [ + build_image_request(source, pixel_format=pixel_format) + for source in img_sources + ] + image_responses = image_client.get_image(image_request) + + for image in image_responses: + num_bytes = 3 + dtype = np.uint8 + # extension = '.jpg' + img = transform(image, dtype, num_bytes) + img = np.rot90(img, k=3) + imgs.append(img) + #black and white depth + elif format == "PIXEL_FORMAT_DEPTH_U16": + pixel_format = pixel_format_string_to_enum(format) + + image_request = [ + build_image_request(source, pixel_format=pixel_format) + for source in depth_sources + ] + image_responses = image_client.get_image(image_request) + + for image in image_responses: + num_bytes = 1 + dtype = np.uint16 + # extension = '.png' + img = transform(image, dtype, num_bytes) + img = np.rot90(img, k=3) + imgs.append(img) + + #colored depth + else: + left_sources = ['frontleft_depth_in_visual_frame', 'frontleft_fisheye_image'] + right_sources = ['frontright_depth_in_visual_frame', 'frontright_fisheye_image'] + + image_client = robot.ensure_client(ImageClient.default_service_name) + + # Capture and save images to disk + image_responses_lst = [image_client.get_image_from_sources(left_sources), image_client.get_image_from_sources(right_sources)] + + for image_responses in image_responses_lst: + # Depth is a raw bytestream + cv_depth = np.frombuffer(image_responses[0].shot.image.data, dtype=np.uint16) + cv_depth = cv_depth.reshape(image_responses[0].shot.image.rows, image_responses[0].shot.image.cols) + + # Visual is a JPEG + cv_visual = cv2.imdecode(np.frombuffer(image_responses[1].shot.image.data, dtype=np.uint8), -1) + + # Convert the visual image from a single channel to RGB so we can add color + visual_rgb = cv_visual if len(cv_visual.shape) == 3 else cv2.cvtColor(cv_visual, cv2.COLOR_GRAY2RGB) + + # Map depth ranges to color + + # cv2.applyColorMap() only supports 8-bit; convert from 16-bit to 8-bit and do scaling + min_val = np.min(cv_depth) + max_val = np.max(cv_depth) + depth_range = max_val - min_val + depth8 = (255.0 / depth_range * (cv_depth - min_val)).astype('uint8') + depth8_rgb = cv2.cvtColor(depth8, cv2.COLOR_GRAY2RGB) + depth_color = cv2.applyColorMap(depth8_rgb, cv2.COLORMAP_JET) + + # Add the two images together. + out = cv2.addWeighted(visual_rgb, 0.5, depth_color, 0.5, 0) + img = np.rot90(out, k=3) + imgs.append(img) + + return imgs + #gripper rgb + else: + image_responses = image_client.get_image_from_sources(['hand_color_image']) + + image = image_responses[0] + if image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: + dtype = np.uint16 + + else: + dtype = np.uint8 + img = np.fromstring(image.shot.image.data, dtype=dtype) + if image.shot.image.format == image_pb2.Image.FORMAT_RAW: + img = img.reshape(image.shot.image.rows, image.shot.image.cols) + else: + img = cv2.imdecode(img, -1) + + return img \ No newline at end of file diff --git a/spot/graph_nav_util.py b/spot/graph_nav_util.py new file mode 100644 index 00000000..61c7b77f --- /dev/null +++ b/spot/graph_nav_util.py @@ -0,0 +1,138 @@ +# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +"""Graph nav utility functions""" + + +def id_to_short_code(id): + """Convert a unique id to a 2 letter short code.""" + tokens = id.split('-') + if len(tokens) > 2: + return '%c%c' % (tokens[0][0], tokens[1][0]) + return None + + +def pretty_print_waypoints(waypoint_id, waypoint_name, short_code_to_count, localization_id): + short_code = id_to_short_code(waypoint_id) + if short_code is None or short_code_to_count[short_code] != 1: + short_code = ' ' # If the short code is not valid/unique, don't show it. + + print( + "%s Waypoint name: %s id: %s short code: %s" % + ('->' if localization_id == waypoint_id else ' ', waypoint_name, waypoint_id, short_code)) + + +def find_unique_waypoint_id(short_code, graph, name_to_id): + """Convert either a 2 letter short code or an annotation name into the associated unique id.""" + if graph is None: + print( + "Please list the waypoints in the map before trying to navigate to a specific one (Option #4)." + ) + return + + if len(short_code) != 2: + # Not a short code, check if it is an annotation name (instead of the waypoint id). + if short_code in name_to_id: + # Short code is a waypoint's annotation name. Check if it is paired with a unique waypoint id. + if name_to_id[short_code] is not None: + # Has an associated waypoint id! + return name_to_id[short_code] + else: + print("The waypoint name %s is used for multiple different unique waypoints. Please use" + \ + "the waypoint id." % (short_code)) + return None + # Also not a waypoint annotation name, so we will operate under the assumption that it is a + # unique waypoint id. + return short_code + + ret = short_code + for waypoint in graph.waypoints: + if short_code == id_to_short_code(waypoint.id): + if ret != short_code: + return short_code # Multiple waypoints with same short code. + ret = waypoint.id + return ret + + +def update_waypoints_and_edges(graph, localization_id, do_print=True): + """Update and print waypoint ids and edge ids.""" + name_to_id = dict() + edges = dict() + + short_code_to_count = {} + waypoint_to_timestamp = [] + for waypoint in graph.waypoints: + # Determine the timestamp that this waypoint was created at. + timestamp = -1.0 + try: + timestamp = waypoint.annotations.creation_time.seconds + waypoint.annotations.creation_time.nanos / 1e9 + except: + # Must be operating on an older graph nav map, since the creation_time is not + # available within the waypoint annotations message. + pass + waypoint_to_timestamp.append((waypoint.id, timestamp, waypoint.annotations.name)) + + # Determine how many waypoints have the same short code. + short_code = id_to_short_code(waypoint.id) + if short_code not in short_code_to_count: + short_code_to_count[short_code] = 0 + short_code_to_count[short_code] += 1 + + # Add the annotation name/id into the current dictionary. + waypoint_name = waypoint.annotations.name + if waypoint_name: + if waypoint_name in name_to_id: + # Waypoint name is used for multiple different waypoints, so set the waypoint id + # in this dictionary to None to avoid confusion between two different waypoints. + name_to_id[waypoint_name] = None + else: + # First time we have seen this waypoint annotation name. Add it into the dictionary + # with the respective waypoint unique id. + name_to_id[waypoint_name] = waypoint.id + + # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, + # fallback to sorting by annotation name. + waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) + + # Print out the waypoints name, id, and short code in an ordered sorted by the timestamp from + # when the waypoint was created. + if do_print: + print('%d waypoints:' % len(graph.waypoints)) + for waypoint in waypoint_to_timestamp: + pretty_print_waypoints(waypoint[0], waypoint[2], short_code_to_count, localization_id) + + for edge in graph.edges: + if edge.id.to_waypoint in edges: + if edge.id.from_waypoint not in edges[edge.id.to_waypoint]: + edges[edge.id.to_waypoint].append(edge.id.from_waypoint) + else: + edges[edge.id.to_waypoint] = [edge.id.from_waypoint] + if do_print: + print("(Edge) from waypoint {} to waypoint {} (cost {})".format( + edge.id.from_waypoint, edge.id.to_waypoint, edge.annotations.cost.value)) + + return name_to_id, edges + + +def sort_waypoints_chrono(graph): + """Sort waypoints by time created.""" + waypoint_to_timestamp = [] + for waypoint in graph.waypoints: + # Determine the timestamp that this waypoint was created at. + timestamp = -1.0 + try: + timestamp = waypoint.annotations.creation_time.seconds + waypoint.annotations.creation_time.nanos / 1e9 + except: + # Must be operating on an older graph nav map, since the creation_time is not + # available within the waypoint annotations message. + pass + waypoint_to_timestamp.append((waypoint.id, timestamp, waypoint.annotations.name)) + + # Sort the set of waypoints by their creation timestamp. If the creation timestamp is unavailable, + # fallback to sorting by annotation name. + waypoint_to_timestamp = sorted(waypoint_to_timestamp, key=lambda x: (x[1], x[2])) + + return waypoint_to_timestamp diff --git a/spot/record_env_graph.py b/spot/record_env_graph.py new file mode 100644 index 00000000..cb1a2a0d --- /dev/null +++ b/spot/record_env_graph.py @@ -0,0 +1,508 @@ +# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +"""Command line interface integrating options to record maps with WASD controls. """ +import argparse +import logging +import os +import sys +import time + +import google.protobuf.timestamp_pb2 +import graph_nav_util +import grpc +from google.protobuf import wrappers_pb2 as wrappers + +import bosdyn.client.channel +import bosdyn.client.util +from bosdyn.api.graph_nav import map_pb2, map_processing_pb2, recording_pb2 +from bosdyn.client import ResponseError, RpcError, create_standard_sdk +from bosdyn.client.graph_nav import GraphNavClient +from bosdyn.client.map_processing import MapProcessingServiceClient +from bosdyn.client.math_helpers import Quat, SE3Pose +from bosdyn.client.recording import GraphNavRecordingServiceClient + + +class RecordingInterface(object): + """Recording service command line interface.""" + + def __init__(self, robot, download_filepath, scene_name, client_metadata): + # Keep the robot instance and it's ID. + self._robot = robot + # Force trigger timesync. + self._robot.time_sync.wait_for_sync() + + + + # Filepath for the location to put the downloaded graph and snapshots. + if download_filepath[-1] == "/": + self._download_filepath = download_filepath + scene_name + else: + self._download_filepath = download_filepath + "/{}".format(scene_name) + + if not os.path.exists(download_filepath): + os.mkdir(download_filepath) + if not os.path.exists(self._download_filepath): + os.mkdir(self._download_filepath) + + # Setup the recording service client. + self._recording_client = self._robot.ensure_client( + GraphNavRecordingServiceClient.default_service_name) + + # Create the recording environment. + self._recording_environment = GraphNavRecordingServiceClient.make_recording_environment( + waypoint_env=GraphNavRecordingServiceClient.make_waypoint_environment( + client_metadata=client_metadata)) + + # Setup the graph nav service client. + self._graph_nav_client = robot.ensure_client(GraphNavClient.default_service_name) + + self._map_processing_client = robot.ensure_client( + MapProcessingServiceClient.default_service_name) + + # Store the most recent knowledge of the state of the robot based on rpc calls. + self._current_graph = None + self._current_edges = dict() #maps to_waypoint to list(from_waypoint) + self._current_waypoint_snapshots = dict() # maps id to waypoint snapshot + self._current_edge_snapshots = dict() # maps id to edge snapshot + self._current_annotation_name_to_wp_id = dict() + + # Add recording service properties to the command line dictionary. + self._command_dictionary = { + '0': self._clear_map, + '1': self._start_recording, + '2': self._stop_recording, + '3': self._get_recording_status, + '4': self._create_default_waypoint, + '5': self._create_named_waypoint, + '6': self._download_full_graph, + '7': self._list_graph_waypoint_and_edge_ids, + '8': self._create_new_edge, + '9': self._create_loop, + '10': self._auto_close_loops_prompt, + 'a': self._optimize_anchoring + } + + def should_we_start_recording(self): + # Before starting to record, check the state of the GraphNav system. + graph = self._graph_nav_client.download_graph() + if graph is not None: + # Check that the graph has waypoints. If it does, then we need to be localized to the graph + # before starting to record + if len(graph.waypoints) > 0: + localization_state = self._graph_nav_client.get_localization_state() + if not localization_state.localization.waypoint_id: + # Not localized to anything in the map. The best option is to clear the graph or + # attempt to localize to the current map. + # Returning false since the GraphNav system is not in the state it should be to + # begin recording. + return False + # If there is no graph or there exists a graph that we are localized to, then it is fine to + # start recording, so we return True. + return True + + def _clear_map(self, *args): + """Clear the state of the map on the robot, removing all waypoints and edges.""" + return self._graph_nav_client.clear_graph() + + def _start_recording(self, *args): + """Start recording a map.""" + should_start_recording = self.should_we_start_recording() + if not should_start_recording: + print("The system is not in the proper state to start recording.", \ + "Try using the graph_nav_command_line to either clear the map or", \ + "attempt to localize to the map.") + return + try: + status = self._recording_client.start_recording( + recording_environment=self._recording_environment) + print("Successfully started recording a map.") + except Exception as err: + print("Start recording failed: " + str(err)) + + def _stop_recording(self, *args): + """Stop or pause recording a map.""" + first_iter = True + while True: + try: + status = self._recording_client.stop_recording() + print("Successfully stopped recording a map.") + break + except bosdyn.client.recording.NotReadyYetError as err: + # It is possible that we are not finished recording yet due to + # background processing. Try again every 1 second. + if first_iter: + print("Cleaning up recording...") + first_iter = False + time.sleep(1.0) + continue + except Exception as err: + print("Stop recording failed: " + str(err)) + break + + def _get_recording_status(self, *args): + """Get the recording service's status.""" + status = self._recording_client.get_record_status() + if status.is_recording: + print("The recording service is on.") + else: + print("The recording service is off.") + + def _create_default_waypoint(self, *args): + """Create a default waypoint at the robot's current location.""" + resp = self._recording_client.create_waypoint(waypoint_name="default") + if resp.status == recording_pb2.CreateWaypointResponse.STATUS_OK: + print("Successfully created a waypoint.") + else: + print("Could not create a waypoint.") + + def _create_named_waypoint(self, *args): + """Create waypoint with specified name at robot's current location""" + print(args[0][0]) + resp = self._recording_client.create_waypoint(waypoint_name=args[0][0]) + + if resp.status == recording_pb2.CreateWaypointResponse.STATUS_OK: + print('Created waypoint for {}'.format(args[0])) + else: + print('Could not create named waypoint') + + def _download_full_graph(self, *args): + """Download the graph and snapshots from the robot.""" + graph = self._graph_nav_client.download_graph() + if graph is None: + print("Failed to download the graph.") + return + self._write_full_graph(graph) + print("Graph downloaded with {} waypoints and {} edges".format( + len(graph.waypoints), len(graph.edges))) + # Download the waypoint and edge snapshots. + self._download_and_write_waypoint_snapshots(graph.waypoints) + self._download_and_write_edge_snapshots(graph.edges) + + def _write_full_graph(self, graph): + """Download the graph from robot to the specified, local filepath location.""" + graph_bytes = graph.SerializeToString() + self._write_bytes(self._download_filepath, '/graph', graph_bytes) + + def _download_and_write_waypoint_snapshots(self, waypoints): + """Download the waypoint snapshots from robot to the specified, local filepath location.""" + num_waypoint_snapshots_downloaded = 0 + for waypoint in waypoints: + if len(waypoint.snapshot_id) == 0: + continue + try: + waypoint_snapshot = self._graph_nav_client.download_waypoint_snapshot( + waypoint.snapshot_id) + except Exception: + # Failure in downloading waypoint snapshot. Continue to next snapshot. + print("Failed to download waypoint snapshot: " + waypoint.snapshot_id) + continue + self._write_bytes(self._download_filepath + '/waypoint_snapshots', + '/' + waypoint.snapshot_id, waypoint_snapshot.SerializeToString()) + num_waypoint_snapshots_downloaded += 1 + print("Downloaded {} of the total {} waypoint snapshots.".format( + num_waypoint_snapshots_downloaded, len(waypoints))) + + def _download_and_write_edge_snapshots(self, edges): + """Download the edge snapshots from robot to the specified, local filepath location.""" + num_edge_snapshots_downloaded = 0 + num_to_download = 0 + for edge in edges: + if len(edge.snapshot_id) == 0: + continue + num_to_download += 1 + try: + edge_snapshot = self._graph_nav_client.download_edge_snapshot(edge.snapshot_id) + except Exception: + # Failure in downloading edge snapshot. Continue to next snapshot. + print("Failed to download edge snapshot: " + edge.snapshot_id) + continue + self._write_bytes(self._download_filepath + '/edge_snapshots', '/' + edge.snapshot_id, + edge_snapshot.SerializeToString()) + num_edge_snapshots_downloaded += 1 + print("Downloaded {} of the total {} edge snapshots.".format( + num_edge_snapshots_downloaded, num_to_download)) + + def _write_bytes(self, filepath, filename, data): + """Write data to a file.""" + os.makedirs(filepath, exist_ok=True) + with open(filepath + filename, 'wb+') as f: + f.write(data) + f.close() + + def _update_graph_waypoint_and_edge_ids(self, do_print=False): + # Download current graph + graph = self._graph_nav_client.download_graph() + if graph is None: + print("Empty graph.") + return + self._current_graph = graph + + localization_id = self._graph_nav_client.get_localization_state().localization.waypoint_id + + # Update and print waypoints and edges + self._current_annotation_name_to_wp_id, self._current_edges = graph_nav_util.update_waypoints_and_edges( + graph, localization_id, do_print) + + def _list_graph_waypoint_and_edge_ids(self, *args): + """List the waypoint ids and edge ids of the graph currently on the robot.""" + self._update_graph_waypoint_and_edge_ids(do_print=True) + + def _create_new_edge(self, *args): + """Create new edge between existing waypoints in map.""" + + if len(args[0]) != 2: + print("ERROR: Specify the two waypoints to connect (short code or annotation).") + return + + self._update_graph_waypoint_and_edge_ids(do_print=False) + + from_id = graph_nav_util.find_unique_waypoint_id(args[0][0], self._current_graph, + self._current_annotation_name_to_wp_id) + to_id = graph_nav_util.find_unique_waypoint_id(args[0][1], self._current_graph, + self._current_annotation_name_to_wp_id) + + print("Creating edge from {} to {}.".format(from_id, to_id)) + + from_wp = self._get_waypoint(from_id) + if from_wp is None: + return + + to_wp = self._get_waypoint(to_id) + if to_wp is None: + return + + # Get edge transform based on kinematic odometry + edge_transform = self._get_transform(from_wp, to_wp) + + # Define new edge + new_edge = map_pb2.Edge() + new_edge.id.from_waypoint = from_id + new_edge.id.to_waypoint = to_id + new_edge.from_tform_to.CopyFrom(edge_transform) + + print("edge transform =", new_edge.from_tform_to) + + # Send request to add edge to map + self._recording_client.create_edge(edge=new_edge) + + def _create_loop(self, *args): + """Create edge from last waypoint to first waypoint.""" + + self._update_graph_waypoint_and_edge_ids(do_print=False) + + if len(self._current_graph.waypoints) < 2: + self._add_message( + "Graph contains {} waypoints -- at least two are needed to create loop.".format( + len(self._current_graph.waypoints))) + return False + + sorted_waypoints = graph_nav_util.sort_waypoints_chrono(self._current_graph) + edge_waypoints = [sorted_waypoints[-1][0], sorted_waypoints[0][0]] + + self._create_new_edge(edge_waypoints) + + def _auto_close_loops_prompt(self, *args): + print(""" + Options: + (0) Close all loops. + (1) Close only fiducial-based loops. + (2) Close only odometry-based loops. + (q) Back. + """) + try: + inputs = input('>') + except NameError: + return + req_type = str.split(inputs)[0] + close_fiducial_loops = False + close_odometry_loops = False + if req_type == '0': + close_fiducial_loops = True + close_odometry_loops = True + elif req_type == '1': + close_fiducial_loops = True + elif req_type == '2': + close_odometry_loops = True + elif req_type == 'q': + return + else: + print("Unrecognized command. Going back.") + return + self._auto_close_loops(close_fiducial_loops, close_odometry_loops) + + def _auto_close_loops(self, close_fiducial_loops, close_odometry_loops, *args): + """Automatically find and close all loops in the graph.""" + response = self._map_processing_client.process_topology( + params=map_processing_pb2.ProcessTopologyRequest.Params( + do_fiducial_loop_closure=wrappers.BoolValue(value=close_fiducial_loops), + do_odometry_loop_closure=wrappers.BoolValue(value=close_odometry_loops)), + modify_map_on_server=True) + print("Created {} new edge(s).".format(len(response.new_subgraph.edges))) + + def _optimize_anchoring(self, *args): + """Call anchoring optimization on the server, producing a globally optimal reference frame for waypoints to be expressed in.""" + response = self._map_processing_client.process_anchoring( + params=map_processing_pb2.ProcessAnchoringRequest.Params(), + modify_anchoring_on_server=True, stream_intermediate_results=False) + if response.status == map_processing_pb2.ProcessAnchoringResponse.STATUS_OK: + print("Optimized anchoring after {} iteration(s).".format(response.iteration)) + else: + print("Error optimizing {}".format(response)) + + def _get_waypoint(self, id): + """Get waypoint from graph (return None if waypoint not found)""" + + if self._current_graph is None: + self._current_graph = self._graph_nav_client.download_graph() + + for waypoint in self._current_graph.waypoints: + if waypoint.id == id: + return waypoint + + print('ERROR: Waypoint {} not found in graph.'.format(id)) + return None + + def _get_transform(self, from_wp, to_wp): + """Get transform from from-waypoint to to-waypoint.""" + + from_se3 = from_wp.waypoint_tform_ko + from_tf = SE3Pose( + from_se3.position.x, from_se3.position.y, from_se3.position.z, + Quat(w=from_se3.rotation.w, x=from_se3.rotation.x, y=from_se3.rotation.y, + z=from_se3.rotation.z)) + + to_se3 = to_wp.waypoint_tform_ko + to_tf = SE3Pose( + to_se3.position.x, to_se3.position.y, to_se3.position.z, + Quat(w=to_se3.rotation.w, x=to_se3.rotation.x, y=to_se3.rotation.y, + z=to_se3.rotation.z)) + + from_T_to = from_tf.mult(to_tf.inverse()) + return from_T_to.to_proto() + + def run_new(self): + #NOTE: ENSURE SPOT IS FACING A FIDUCIAL WHEN YOU START RUNNING THIS!!! + while True: + print(""" + Options: + (0) Clear map. + (1) Start recording a map. + (2) Stop recording a map. + (3) Get the recording service's status. + (4) Create a default waypoint in the current robot's location. + (5) Create a named waypoint in the current robot's location + (6) Download the map after recording. + (7) List the waypoint ids and edge ids of the map on the robot. + (8) Create new edge between existing waypoints using odometry. + (9) Create new edge from last waypoint to first waypoint using odometry. + (10) Automatically find and close loops. + (a) Optimize the map's anchoring. + (q) Exit. + """) + try: + inputs = input('>') + except NameError: + pass + req_type = inputs.split(' ')[0] + + args = None + if len(inputs.split(' ')) > 1: + args = inputs.split(' ')[1:] + + if req_type == 'q': + print('Quitting data collection for scene graph') + break + + if req_type not in self._command_dictionary: + print("Request not in the known command dictionary.") + continue + + if req_type == '5' and args is None: + print('Add argument for the waypoint name after the command option') + continue + + try: + #add to saved waypoints if new waypoint generated + cmd_func = self._command_dictionary[req_type] + cmd_func(args) + except Exception as e: + print('Error: ', e) + continue + + def run(self): + """Main loop for the command line interface.""" + while True: + print(""" + Options: + (0) Clear map. + (1) Start recording a map. + (2) Stop recording a map. + (3) Get the recording service's status. + (4) Create a default waypoint in the current robot's location. + (5) Download the map after recording. + (6) List the waypoint ids and edge ids of the map on the robot. + (7) Create new edge between existing waypoints using odometry. + (8) Create new edge from last waypoint to first waypoint using odometry. + (9) Automatically find and close loops. + (a) Optimize the map's anchoring. + (q) Exit. + """) + try: + inputs = input('>') + except NameError: + pass + req_type = str.split(inputs)[0] + + if req_type == 'q': + break + + if req_type not in self._command_dictionary: + print("Request not in the known command dictionary.") + continue + try: + cmd_func = self._command_dictionary[req_type] + cmd_func(str.split(inputs)[1:]) + except Exception as e: + print(e) + + +def main(argv): + """Run the command-line interface.""" + + HOSTNAME = 'gouger' + IP = "138.16.161.23" + USER = 'user' + PASS = 'bigbubbabigbubba' + DOWNLOAD_ROOT = './Trajectories' + + # Create robot object. + sdk = bosdyn.client.create_standard_sdk('RecordingClient') + robot = sdk.create_robot(IP) + robot.authenticate(USER, PASS) + + user_name = robot._current_user + session_name = DOWNLOAD_ROOT + + # Crate metadata for the recording session. + client_metadata = GraphNavRecordingServiceClient.make_client_metadata( + session_name=session_name, client_username=user_name, client_id='RecordingClient', + client_type='Python SDK') + recording_command_line = RecordingInterface(robot, DOWNLOAD_ROOT, 'demo', client_metadata) + + try: + recording_command_line.run_new() + return True + except Exception as exc: # pylint: disable=broad-except + print(exc) + print("Recording command line client threw an error.") + return False + + +if __name__ == '__main__': + if not main(sys.argv[1:]): + sys.exit(1) + pass diff --git a/spot/robot_planning_env.txt b/spot/robot_planning_env.txt new file mode 100644 index 00000000..6ef1f5ac --- /dev/null +++ b/spot/robot_planning_env.txt @@ -0,0 +1,72 @@ +# This file may be used to create an environment using: +# $ conda create --name --file +# platform: linux-64 +_libgcc_mutex=0.1=main +_openmp_mutex=5.1=1_gnu +absl-py=1.4.0=pypi_0 +addict=2.4.0=pypi_0 +aiofiles=0.8.0=pypi_0 +aiosqlite=0.17.0=pypi_0 +anyio=3.6.2=pypi_0 +bosdyn-api=3.2.3=pypi_0 +bosdyn-choreography-client=3.2.3=pypi_0 +bosdyn-choreography-protos=3.2.3=pypi_0 +bosdyn-client=3.2.3=pypi_0 +bosdyn-core=3.2.3=pypi_0 +bosdyn-mission=3.2.3=pypi_0 +ca-certificates=2023.05.30=h06a4308_0 +certifi=2021.5.30=py36h06a4308_0 +charset-normalizer=2.0.12=pypi_0 +contextvars=2.4=pypi_0 +cycler=0.11.0=pypi_0 +dataclasses=0.8=pypi_0 +decorator=4.4.2=pypi_0 +deprecated=1.2.14=pypi_0 +fancycompleter=0.9.1=pypi_0 +grpcio=1.48.2=pypi_0 +idna=3.4=pypi_0 +immutables=0.19=pypi_0 +importlib-resources=5.4.0=pypi_0 +intel-openmp=2021.4.0=h06a4308_3561 +kiwisolver=1.3.1=pypi_0 +ld_impl_linux-64=2.38=h1181459_1 +libffi=3.3=he6710b0_2 +libgcc-ng=11.2.0=h1234567_1 +libgomp=11.2.0=h1234567_1 +libstdcxx-ng=11.2.0=h1234567_1 +matplotlib=3.3.4=pypi_0 +mkl=2021.4.0=h06a4308_640 +mkl-service=2.4.0=py36h7f8727e_0 +ncurses=6.4=h6a678d5_0 +networkx=2.5.1=pypi_0 +numpy=1.19.5=pypi_0 +openssl=1.1.1t=h7f8727e_0 +pdbpp=0.10.3=pypi_0 +pillow=8.4.0=pypi_0 +pip=21.2.2=py36h06a4308_0 +protobuf=3.19.6=pypi_0 +pygments=2.14.0=pypi_0 +pyjwt=2.4.0=pypi_0 +pyparsing=3.1.1=pypi_0 +pyrepl=0.9.0=pypi_0 +python=3.6.13=h12debd9_1 +python-dateutil=2.8.2=pypi_0 +readline=8.2=h5eee18b_0 +requests=2.27.1=pypi_0 +setuptools=58.0.4=py36h06a4308_0 +six=1.16.0=pyhd3eb1b0_1 +sniffio=1.2.0=pypi_0 +sqlite=3.41.2=h5eee18b_0 +tk=8.6.12=h1ccaba5_0 +torch=1.10.2=pypi_0 +tqdm=4.64.1=pypi_0 +transformations=2021.6.6=pypi_0 +typing-extensions=4.1.1=pypi_0 +urllib3=1.26.16=pypi_0 +wheel=0.37.1=pyhd3eb1b0_0 +wmctrl=0.4=pypi_0 +wrapt=1.15.0=pypi_0 +xz=5.4.2=h5eee18b_0 +zipp=3.6.0=pypi_0 +zlib=1.2.13=h5eee18b_0 +zstd=1.5.2.0=pypi_0 diff --git a/spot/shader_frag.glsl b/spot/shader_frag.glsl new file mode 100644 index 00000000..ab00f2ba --- /dev/null +++ b/spot/shader_frag.glsl @@ -0,0 +1,79 @@ +#version 130 + +// Color the projection plane should be in regions that no images cover. +uniform vec4 backColor; + +varying vec4 position_wrt_camera1_mvp; +uniform sampler2D image1; + +varying vec4 position_wrt_camera2_mvp; +uniform sampler2D image2; + +// When to images overlap, we blend them together. blendingPower controls how aggresive the +// blending window is. blendingPower of Infinity would be no blending (hard cut line between +// images). blendingPower of 0 would mean equal weights always applied to information from both +// cameras. blendingMinimum is more of a helper number to prevent numerial overflow. +const float blendingMinimum = 0.001; +const float blendingPower = 10.0; + +// How far was this point from the center of the camera image?? This will be used to determine +// how the information retrieved from the image will be blended with other images. +float calculateScore(vec2 imageLocation) { + vec2 locationNormalized = imageLocation * 2.0 - 1.0; // -1 to 1 (left to right up to down) + locationNormalized = 1.0 - abs(locationNormalized); // 0.01s on border, 1s in centers + if (min(locationNormalized.x, locationNormalized.y) < 0.0) { + return 0.0; + } + + float score = mix(locationNormalized.x * locationNormalized.y, 1.0, blendingMinimum); + // 0.001 on border, 1.0 in middle + // contour lines are ellipses + + return pow(score, blendingPower); +} + +vec2 calculateImageLocation(vec4 xyz1, out float weight) { + if (xyz1.z <= 0.0) { + // Behind camera! + weight = 0.0; + return vec2(0.0, 0.0); + } else { + // When you multiply an XYZ location by a projection matrix, you need to divide resultant X and Y + // by Z to get pixel coordinates U and V + vec2 location = (xyz1.xy / xyz1.z); // should return pixel coordinate u v on scale of 0 to 1 + + weight = calculateScore(location); + return location; + } +} + +void main() +{ + float totalWeight = 0.0; + + float image1Weight; + vec2 image1Location = calculateImageLocation(position_wrt_camera1_mvp, image1Weight); + vec4 image1Color = texture(image1, image1Location); + totalWeight += image1Weight; + + float image2Weight; + vec2 image2Location = calculateImageLocation(position_wrt_camera2_mvp, image2Weight); + vec4 image2Color = texture(image2, image2Location); + totalWeight += image2Weight; + + if (totalWeight == 0.0) { + + gl_FragColor = backColor; + + } else { + + gl_FragColor = vec4(0.0); + + image1Weight /= totalWeight; + gl_FragColor += image1Color * image1Weight; + + image2Weight /= totalWeight; + gl_FragColor += image2Color * image2Weight; + + } +} \ No newline at end of file diff --git a/spot/shader_vert.glsl b/spot/shader_vert.glsl new file mode 100644 index 00000000..4f3500dd --- /dev/null +++ b/spot/shader_vert.glsl @@ -0,0 +1,16 @@ +#version 130 + +uniform mat4 camera1_MVP; +varying vec4 position_wrt_camera1_mvp; + +uniform mat4 camera2_MVP; +varying vec4 position_wrt_camera2_mvp; + +void main() { + gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex; + + vec4 position_wrt_vo = gl_Vertex; + + position_wrt_camera1_mvp = camera1_MVP * position_wrt_vo; + position_wrt_camera2_mvp = camera2_MVP * position_wrt_vo; +} \ No newline at end of file diff --git a/spot/spot_data.json b/spot/spot_data.json new file mode 100644 index 00000000..6603dbc5 --- /dev/null +++ b/spot/spot_data.json @@ -0,0 +1,2705 @@ +[ + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.046297602355480194, + "y": -0.032882098108530045, + "z": 0.010477404110133648 + }, + "angular": { + "x": 0.004930482245981693, + "y": -0.019209135323762894, + "z": -0.0017601826693862677 + } + }, + "body_state_vision": { + "linear": { + "x": 0.055599480867385864, + "y": -0.011549819260835648, + "z": 0.01047741062939167 + }, + "angular": { + "x": 0.010720742866396904, + "y": 0.016684304922819138, + "z": -0.0017601827858015895 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.05706773325800896, + "y": -0.02925274521112442, + "z": -0.008542671799659729 + }, + "angular": { + "x": 0.027991313487291336, + "y": -0.01183178462088108, + "z": -0.03399800509214401 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.06026884913444519, + "y": -0.021911507472395897, + "z": -0.008542663417756557 + }, + "angular": { + "x": -0.010377546772360802, + "y": 0.028562404215335846, + "z": -0.03399800509214401 + } + }, + "gripper_open_percent": 0.31567811965942383, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.1932777464389801, + "y": -0.2524638772010803, + "z": -0.010860007256269455 + }, + "angular": { + "x": -0.027660634368658066, + "y": 0.04496874660253525, + "z": -0.020262371748685837 + } + }, + "body_state_vision": { + "linear": { + "x": 0.3165188133716583, + "y": 0.030168645083904266, + "z": -0.010860027745366096 + }, + "angular": { + "x": -0.014129172079265118, + "y": -0.050869084894657135, + "z": -0.020262369886040688 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.19176465272903442, + "y": -0.23362964391708374, + "z": 0.021446608006954193 + }, + "angular": { + "x": -0.03725293278694153, + "y": 0.0701647698879242, + "z": -0.03720131143927574 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.301688015460968, + "y": 0.018461108207702637, + "z": 0.021446578204631805 + }, + "angular": { + "x": -0.026064636185765266, + "y": -0.07504335790872574, + "z": -0.03720130771398544 + } + }, + "gripper_open_percent": 0.3133833408355713, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.057912252843379974, + "y": -0.17434385418891907, + "z": 0.0042699286714196205 + }, + "angular": { + "x": -0.09967358410358429, + "y": -0.043815337121486664, + "z": 0.4467518925666809 + } + }, + "body_state_vision": { + "linear": { + "x": 0.08834539353847504, + "y": 0.16107358038425446, + "z": 0.004269951023161411 + }, + "angular": { + "x": 0.09993220865726471, + "y": -0.04322216659784317, + "z": 0.4467518925666809 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.2618231177330017, + "y": -0.3112160265445709, + "z": 0.028258265927433968 + }, + "angular": { + "x": -0.08450005948543549, + "y": -0.06254488974809647, + "z": 0.5399317145347595 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.04988382011651993, + "y": 0.40363141894340515, + "z": 0.028258293867111206 + }, + "angular": { + "x": 0.10333134979009628, + "y": -0.019358424469828606, + "z": 0.5399317145347595 + } + }, + "gripper_open_percent": 0.3103315830230713, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.06990043073892593, + "y": 0.05088625103235245, + "z": 0.04209630936384201 + }, + "angular": { + "x": -0.02627091482281685, + "y": -0.029406443238258362, + "z": 0.032218851149082184 + } + }, + "body_state_vision": { + "linear": { + "x": 0.010277960449457169, + "y": -0.08584784716367722, + "z": 0.04209630563855171 + }, + "angular": { + "x": 0.03942490741610527, + "y": 0.0007599871605634689, + "z": 0.032218851149082184 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.04066631197929382, + "y": 0.04473748058080673, + "z": 0.05934147164225578 + }, + "angular": { + "x": -0.10338766872882843, + "y": -0.12193383276462555, + "z": 0.14123158156871796 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.005109623074531555, + "y": -0.0602419339120388, + "z": 0.059341467916965485 + }, + "angular": { + "x": 0.159702330827713, + "y": 0.007214263081550598, + "z": 0.14123158156871796 + } + }, + "gripper_open_percent": 0.33628344535827637, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.015459403395652771, + "y": -0.013445202261209488, + "z": 0.0017019907245412469 + }, + "angular": { + "x": 0.046132951974868774, + "y": 0.076747365295887, + "z": 0.05857548862695694 + } + }, + "body_state_vision": { + "linear": { + "x": 0.020372020080685616, + "y": -0.002179003320634365, + "z": 0.0017020016675814986 + }, + "angular": { + "x": -0.08763052523136139, + "y": -0.018420211970806122, + "z": 0.05857548862695694 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.007904911413788795, + "y": -0.021487975493073463, + "z": -0.012518811970949173 + }, + "angular": { + "x": 0.07371965050697327, + "y": 0.09675886482000351, + "z": -0.17428578436374664 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.021124832332134247, + "y": 0.008829629980027676, + "z": -0.012518806383013725 + }, + "angular": { + "x": -0.12106645852327347, + "y": -0.011822983622550964, + "z": -0.17428578436374664 + } + }, + "gripper_open_percent": 0.31261444091796875, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.1694914698600769, + "y": -0.2029930204153061, + "z": 0.007018969394266605 + }, + "angular": { + "x": 0.06727626919746399, + "y": 0.2556704878807068, + "z": 0.05236000567674637 + } + }, + "body_state_vision": { + "linear": { + "x": 0.2640821039676666, + "y": 0.013934619724750519, + "z": 0.0070189679972827435 + }, + "angular": { + "x": -0.233127161860466, + "y": -0.1246805340051651, + "z": 0.05236000567674637 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.07493068277835846, + "y": -0.22574718296527863, + "z": 0.05247914418578148 + }, + "angular": { + "x": 0.014539361000061035, + "y": 0.26562559604644775, + "z": 0.0425860621035099 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.21640899777412415, + "y": 0.09870938211679459, + "z": 0.05247912183403969 + }, + "angular": { + "x": -0.2045356184244156, + "y": -0.17009860277175903, + "z": 0.0425860621035099 + } + }, + "gripper_open_percent": 0.35994648933410645, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.022216694429516792, + "y": 0.000489579513669014, + "z": 0.04082881659269333 + }, + "angular": { + "x": -0.08329374343156815, + "y": -0.023520108312368393, + "z": -0.05704572796821594 + } + }, + "body_state_vision": { + "linear": { + "x": -0.015476718544960022, + "y": 0.015946509316563606, + "z": 0.04082884639501572 + }, + "angular": { + "x": 0.07391437143087387, + "y": -0.04503002390265465, + "z": -0.05704572796821594 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.03262931481003761, + "y": 0.04165013134479523, + "z": 0.1081274002790451 + }, + "angular": { + "x": -0.11653847992420197, + "y": 0.08443114161491394, + "z": -0.22594603896141052 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.008316302672028542, + "y": -0.052251748740673065, + "z": 0.10812743008136749 + }, + "angular": { + "x": 0.01743350178003311, + "y": -0.14284926652908325, + "z": -0.22594603896141052 + } + }, + "gripper_open_percent": 0.3431558609008789, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.005419701337814331, + "y": -0.10119960457086563, + "z": 0.0011256752768531442 + }, + "angular": { + "x": 0.012015443295240402, + "y": 0.046467822045087814, + "z": 0.011064117774367332 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0778438001871109, + "y": 0.06489280611276627, + "z": 0.001125670038163662 + }, + "angular": { + "x": -0.04222637414932251, + "y": -0.022815831005573273, + "z": 0.011064117774367332 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.013839513063430786, + "y": -0.10686171799898148, + "z": 0.0018132140394300222 + }, + "angular": { + "x": 0.019136767834424973, + "y": 0.05540051311254501, + "z": 0.018434472382068634 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.06888730823993683, + "y": 0.08285827934741974, + "z": 0.0018132070545107126 + }, + "angular": { + "x": -0.05361785739660263, + "y": -0.023676065728068352, + "z": 0.018434472382068634 + } + }, + "gripper_open_percent": 0.31261444091796875, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0034804195165634155, + "y": -0.21467508375644684, + "z": 0.03108731471002102 + }, + "angular": { + "x": 0.046894755214452744, + "y": 0.016054658219218254, + "z": 0.023413512855768204 + } + }, + "body_state_vision": { + "linear": { + "x": 0.15967532992362976, + "y": 0.14353153109550476, + "z": 0.03108729049563408 + }, + "angular": { + "x": -0.04367522895336151, + "y": 0.0234381016343832, + "z": 0.023413516581058502 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0714869499206543, + "y": -0.1964424103498459, + "z": -0.15671192109584808 + }, + "angular": { + "x": 0.10898944735527039, + "y": 0.14297270774841309, + "z": -0.42593497037887573 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.19259199500083923, + "y": 0.08129158616065979, + "z": -0.15671195089817047 + }, + "angular": { + "x": -0.1789308488368988, + "y": -0.017425939440727234, + "z": -0.42593497037887573 + } + }, + "gripper_open_percent": 93.07662963867188, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.02321997657418251, + "y": -0.016352985054254532, + "z": 0.003129191230982542 + }, + "angular": { + "x": -0.011563912034034729, + "y": -0.03151204064488411, + "z": 0.044288214296102524 + } + }, + "body_state_vision": { + "linear": { + "x": 0.02778363972902298, + "y": -0.005886995233595371, + "z": 0.0031292056664824486 + }, + "angular": { + "x": 0.03096000663936138, + "y": 0.012969614937901497, + "z": 0.044288214296102524 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.015291229821741581, + "y": -0.01180165633559227, + "z": 0.022311653941869736 + }, + "angular": { + "x": -0.04737930744886398, + "y": -0.021353386342525482, + "z": 0.040757790207862854 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.01905323937535286, + "y": -0.003174152225255966, + "z": 0.022311674430966377 + }, + "angular": { + "x": 0.04788772016763687, + "y": -0.0201874952763319, + "z": 0.040757786482572556 + } + }, + "gripper_open_percent": 97.74284362792969, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.02090587094426155, + "y": 0.031594473868608475, + "z": 0.002992029767483473 + }, + "angular": { + "x": -0.033710382878780365, + "y": -0.003912564367055893, + "z": -0.05462237820029259 + } + }, + "body_state_vision": { + "linear": { + "x": -0.008925413712859154, + "y": -0.0368184819817543, + "z": 0.0029920446686446667 + }, + "angular": { + "x": 0.025806212797760963, + "y": -0.022039473056793213, + "z": -0.05462237820029259 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.19558528065681458, + "y": -0.03286460041999817, + "z": -0.030478529632091522 + }, + "angular": { + "x": -0.024181008338928223, + "y": 0.012472633272409439, + "z": 0.22310864925384521 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.1571737825870514, + "y": -0.12095482647418976, + "z": -0.030478529632091522 + }, + "angular": { + "x": 0.0073151737451553345, + "y": -0.026206448674201965, + "z": 0.22310864925384521 + } + }, + "gripper_open_percent": 97.75582122802734, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0199153870344162, + "y": -0.16305775940418243, + "z": -0.028179587796330452 + }, + "angular": { + "x": -0.0528729110956192, + "y": -0.0635678619146347, + "z": 0.015093043446540833 + } + }, + "body_state_vision": { + "linear": { + "x": 0.13303546607494354, + "y": 0.09636399149894714, + "z": -0.028179580345749855 + }, + "angular": { + "x": 0.08255937695503235, + "y": 0.004513021558523178, + "z": 0.015093043446540833 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.05893699452280998, + "y": -0.09981504827737808, + "z": -0.038803234696388245 + }, + "angular": { + "x": -0.006010785698890686, + "y": -0.25786876678466797, + "z": 0.06308582425117493 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.03303587809205055, + "y": 0.11110913753509521, + "z": -0.03880324959754944 + }, + "angular": { + "x": 0.19304820895195007, + "y": 0.17106972634792328, + "z": 0.06308579444885254 + } + }, + "gripper_open_percent": 97.7382583618164, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.09457845985889435, + "y": -0.04330688714981079, + "z": 0.0035154763609170914 + }, + "angular": { + "x": -0.014303043484687805, + "y": 0.14889346063137054, + "z": 0.03885262459516525 + } + }, + "body_state_vision": { + "linear": { + "x": -0.03262476623058319, + "y": 0.09877345710992813, + "z": 0.0035155327059328556 + }, + "angular": { + "x": -0.09937141835689545, + "y": -0.1117996871471405, + "z": 0.03885262832045555 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.02676945924758911, + "y": -0.10020607709884644, + "z": -0.3580649495124817 + }, + "angular": { + "x": -0.0591045618057251, + "y": -0.18658152222633362, + "z": -0.8054566383361816 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.05521169304847717, + "y": 0.0878039300441742, + "z": -0.3580648601055145 + }, + "angular": { + "x": 0.17694032192230225, + "y": 0.08365488052368164, + "z": -0.8054565787315369 + } + }, + "gripper_open_percent": 97.74894714355469, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.021012261509895325, + "y": -0.04349793493747711, + "z": -0.015453170984983444 + }, + "angular": { + "x": 0.10821841657161713, + "y": 0.029517561197280884, + "z": -0.0005394807667471468 + } + }, + "body_state_vision": { + "linear": { + "x": 0.046172283589839935, + "y": 0.01420232467353344, + "z": -0.015453245490789413 + }, + "angular": { + "x": -0.09526988118886948, + "y": 0.059212878346443176, + "z": -0.0005394807667471468 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.12223398685455322, + "y": 0.14682501554489136, + "z": -0.45771872997283936 + }, + "angular": { + "x": 0.10436636209487915, + "y": 0.2878383696079254, + "z": -0.5955639481544495 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.02441108226776123, + "y": -0.18948030471801758, + "z": -0.4577189087867737 + }, + "angular": { + "x": -0.28193795680999756, + "y": -0.11939136683940887, + "z": -0.5955640077590942 + } + }, + "gripper_open_percent": 97.67108917236328, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.00507016945630312, + "y": -0.002234536921605468, + "z": 0.0009559177560731769 + }, + "angular": { + "x": -0.0042343176901340485, + "y": 0.005390350706875324, + "z": 0.020203113555908203 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0050875283777713776, + "y": -0.0021947191562503576, + "z": 0.0009559215395711362 + }, + "angular": { + "x": -0.0010685105808079243, + "y": -0.006770791951566935, + "z": 0.020203113555908203 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.05357719957828522, + "y": -0.05284674093127251, + "z": -0.15614908933639526 + }, + "angular": { + "x": 0.005793208256363869, + "y": -0.006497031077742577, + "z": 0.012748686596751213 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0751824676990509, + "y": -0.0032985592260956764, + "z": -0.15614914894104004 + }, + "angular": { + "x": 0.0008186623454093933, + "y": 0.008666165173053741, + "z": 0.012748690322041512 + } + }, + "gripper_open_percent": 97.74665832519531, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0035833031870424747, + "y": -0.0029270180966705084, + "z": -0.004045411944389343 + }, + "angular": { + "x": -0.011992641724646091, + "y": -0.0030425088480114937, + "z": 0.05305201932787895 + } + }, + "body_state_vision": { + "linear": { + "x": 0.004583201836794615, + "y": -0.0006339722312986851, + "z": -0.0040454138070344925 + }, + "angular": { + "x": 0.01039019413292408, + "y": -0.006717458833009005, + "z": 0.05305201932787895 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.002558761741966009, + "y": -0.0068041314370930195, + "z": 0.0004463748773559928 + }, + "angular": { + "x": 0.0038698166608810425, + "y": -0.004376955330371857, + "z": 0.12007707357406616 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.003244677558541298, + "y": 0.0065050283446908, + "z": 0.00044637429527938366 + }, + "angular": { + "x": 0.0005739778280258179, + "y": 0.005814090371131897, + "z": 0.12007705867290497 + } + }, + "gripper_open_percent": 14.012802124023438, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.002487013814970851, + "y": 0.0034313295036554337, + "z": -0.0008729544933885336 + }, + "angular": { + "x": -0.0009032473899424076, + "y": -0.004134719725698233, + "z": -0.008606260642409325 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0008220074232667685, + "y": -0.0041573490016162395, + "z": -0.0008729550172574818 + }, + "angular": { + "x": 0.0036444298457354307, + "y": 0.0021517195273190737, + "z": -0.008606260642409325 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0022492092102766037, + "y": 0.001828196458518505, + "z": 0.0005509090842679143 + }, + "angular": { + "x": -0.007858123630285263, + "y": -0.002816143911331892, + "z": 0.002536522690206766 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.00019089557463303208, + "y": -0.0028921952471137047, + "z": 0.000550907920114696 + }, + "angular": { + "x": 0.0074108662083745, + "y": -0.00384185160510242, + "z": 0.0025365217588841915 + } + }, + "gripper_open_percent": 8.774858474731445, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.00015389121836051345, + "y": -0.00012138960300944746, + "z": 0.00021118881704751402 + }, + "angular": { + "x": -0.0021094689145684242, + "y": 0.0020609658677130938, + "z": 0.00015321228420361876 + } + }, + "body_state_vision": { + "linear": { + "x": -1.5769335732329637e-05, + "y": 0.00019536954641807824, + "z": 0.00021119254233781248 + }, + "angular": { + "x": -7.476052269339561e-05, + "y": -0.0029481961391866207, + "z": 0.00015321228420361876 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0012884698808193207, + "y": -0.00031217531068250537, + "z": 0.0009117945446632802 + }, + "angular": { + "x": -0.003553292015567422, + "y": 0.0031425333581864834, + "z": -0.0007544553373008966 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0006480240263044834, + "y": 0.0011565778404474258, + "z": 0.0009117990266531706 + }, + "angular": { + "x": 0.000115192960947752, + "y": -0.00474216602742672, + "z": -0.0007544555701315403 + } + }, + "gripper_open_percent": 8.489370346069336, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0013953747693449259, + "y": -0.0004965175176039338, + "z": -0.00015519102453254163 + }, + "angular": { + "x": -0.001368107507005334, + "y": 0.0011146360775455832, + "z": -0.004363197833299637 + } + }, + "body_state_vision": { + "linear": { + "x": 0.001313357730396092, + "y": -0.0006846156902611256, + "z": -0.00015519149019382894 + }, + "angular": { + "x": 0.00011419912334531546, + "y": -0.001760991057381034, + "z": -0.004363197833299637 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0023250195663422346, + "y": -0.00290656671859324, + "z": 0.0017797971377149224 + }, + "angular": { + "x": -0.0002226661890745163, + "y": -0.0004363411571830511, + "z": 0.0053648012690246105 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0037119670305401087, + "y": 0.00027415785007178783, + "z": 0.0017797964392229915 + }, + "angular": { + "x": 0.00047125574201345444, + "y": 0.0001337581779807806, + "z": 0.005364802200347185 + } + }, + "gripper_open_percent": 8.469522476196289, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0002718915056902915, + "y": -2.4597044102847576e-05, + "z": 0.0001046897450578399 + }, + "angular": { + "x": -0.002415148541331291, + "y": 0.002669012639671564, + "z": -0.003931315615773201 + } + }, + "body_state_vision": { + "linear": { + "x": 0.00020304210192989558, + "y": -0.0001824945502448827, + "z": 0.0001046906691044569 + }, + "angular": { + "x": -0.000312308082357049, + "y": -0.003585950005799532, + "z": -0.003931315615773201 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.014461941085755825, + "y": -0.005541772581636906, + "z": 0.0060295239090919495 + }, + "angular": { + "x": -0.020770017057657242, + "y": 0.0015754606574773788, + "z": -0.04446963593363762 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.013901881873607635, + "y": -0.006826183758676052, + "z": 0.006029524840414524 + }, + "angular": { + "x": 0.012979093939065933, + "y": -0.016291650012135506, + "z": -0.044469643384218216 + } + }, + "gripper_open_percent": 8.455789566040039, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.00012832562788389623, + "y": 0.0002958674740511924, + "z": -0.014275673776865005 + }, + "angular": { + "x": 0.06395264714956284, + "y": -0.013471737504005432, + "z": -0.001807608874514699 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0001294788089580834, + "y": -0.00029536470538005233, + "z": -0.014275673776865005 + }, + "angular": { + "x": -0.033646754920482635, + "y": 0.056029655039310455, + "z": -0.0018076091073453426 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.01346251368522644, + "y": 0.013158340007066727, + "z": 0.21052725613117218 + }, + "angular": { + "x": 0.011310845613479614, + "y": -0.04429998621344566, + "z": 0.05423645302653313 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.018802911043167114, + "y": 0.0009109452366828918, + "z": 0.21052728593349457 + }, + "angular": { + "x": 0.02476484328508377, + "y": 0.038433417677879333, + "z": 0.05423644930124283 + } + }, + "gripper_open_percent": 8.435172080993652, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0004827622324228287, + "y": -0.013808379881083965, + "z": 0.008308072574436665 + }, + "angular": { + "x": 0.016871687024831772, + "y": -0.0174611397087574, + "z": -0.04085422679781914 + } + }, + "body_state_vision": { + "linear": { + "x": 0.010446848347783089, + "y": 0.009042598307132721, + "z": 0.008308064192533493 + }, + "angular": { + "x": 0.0013141371309757233, + "y": 0.024244969710707664, + "z": -0.04085422679781914 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0263708233833313, + "y": 0.013675175607204437, + "z": 0.40131688117980957 + }, + "angular": { + "x": 0.09344496577978134, + "y": 0.06173274666070938, + "z": 0.09681390225887299 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.027965396642684937, + "y": 0.010018005967140198, + "z": 0.4013168215751648 + }, + "angular": { + "x": -0.10882304608821869, + "y": 0.026465628296136856, + "z": 0.09681390225887299 + } + }, + "gripper_open_percent": 7.942831516265869, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.003783542662858963, + "y": 0.0019090082496404648, + "z": 0.0008372155716642737 + }, + "angular": { + "x": -2.1552201360464096e-05, + "y": -0.007048267871141434, + "z": -0.00779487332329154 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0011757609900087118, + "y": -0.004071493167430162, + "z": 0.0008372173761017621 + }, + "angular": { + "x": 0.0051794094033539295, + "y": 0.004780404735356569, + "z": -0.00779487332329154 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0053795501589775085, + "y": 0.011665208265185356, + "z": 0.08059846609830856 + }, + "angular": { + "x": 0.04166411608457565, + "y": 0.008195165544748306, + "z": 0.035687677562236786 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.012208551168441772, + "y": -0.003995964303612709, + "z": 0.08059848845005035 + }, + "angular": { + "x": -0.03435670584440231, + "y": 0.02495349571108818, + "z": 0.03568769246339798 + } + }, + "gripper_open_percent": 7.9252777099609375, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0004060811479575932, + "y": -0.00018789767636917531, + "z": 0.00031923403730615973 + }, + "angular": { + "x": -0.0002834178740158677, + "y": -0.0008490367908962071, + "z": -0.0020383400842547417 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0004140146484132856, + "y": -0.00016970181604847312, + "z": 0.0003192335134372115 + }, + "angular": { + "x": 0.0008150067878887057, + "y": 0.0003700723173096776, + "z": -0.0020383400842547417 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.005786115303635597, + "y": 0.011953050270676613, + "z": 0.02332284301519394 + }, + "angular": { + "x": 0.05295542627573013, + "y": -0.005240479484200478, + "z": -0.0035567060112953186 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.012696142308413982, + "y": -0.0038939162623137236, + "z": 0.02332284301519394 + }, + "angular": { + "x": -0.032194994390010834, + "y": 0.04237006604671478, + "z": -0.0035567060112953186 + } + }, + "gripper_open_percent": 7.913827896118164, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.02012808434665203, + "y": -0.004358397796750069, + "z": -0.0010508191771805286 + }, + "angular": { + "x": 0.017453467473387718, + "y": -0.0415349155664444, + "z": -0.24297791719436646 + } + }, + "body_state_vision": { + "linear": { + "x": -0.01050317008048296, + "y": 0.017715374007821083, + "z": -0.0010508635314181447 + }, + "angular": { + "x": 0.01855873316526413, + "y": 0.041052982211112976, + "z": -0.24297791719436646 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.022627580910921097, + "y": 0.008889365941286087, + "z": 0.01588680036365986 + }, + "angular": { + "x": -0.03713410347700119, + "y": -0.12464950233697891, + "z": 0.10358038544654846 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.008883636444807053, + "y": -0.02262943424284458, + "z": 0.015886753797531128 + }, + "angular": { + "x": 0.11660817265510559, + "y": 0.057610683143138885, + "z": 0.10358037799596786 + } + }, + "gripper_open_percent": 7.896268367767334, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.02853313833475113, + "y": 0.049634285271167755, + "z": 0.01704792119562626 + }, + "angular": { + "x": 0.02102210745215416, + "y": 0.014503180980682373, + "z": -0.2560817003250122 + } + }, + "body_state_vision": { + "linear": { + "x": -0.05578649044036865, + "y": -0.01286698505282402, + "z": 0.01704791747033596 + }, + "angular": { + "x": -0.02493254840373993, + "y": 0.005535200238227844, + "z": -0.2560817003250122 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.18820706009864807, + "y": 0.14204704761505127, + "z": -0.0037740617990493774 + }, + "angular": { + "x": 0.050132185220718384, + "y": 0.016921989619731903, + "z": -0.3486379384994507 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.02398347109556198, + "y": -0.23457218706607819, + "z": -0.0037740767002105713 + }, + "angular": { + "x": -0.04651379585266113, + "y": 0.025220192968845367, + "z": -0.34863799810409546 + } + }, + "gripper_open_percent": 7.832914352416992, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.004644172266125679, + "y": -0.019054310396313667, + "z": 0.0008909606258384883 + }, + "angular": { + "x": -0.07721823453903198, + "y": -0.15394006669521332, + "z": -0.44398683309555054 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0171226654201746, + "y": 0.009562959894537926, + "z": 0.0008910198230296373 + }, + "angular": { + "x": 0.16534771025180817, + "y": 0.04816989600658417, + "z": -0.44398683309555054 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.3073084354400635, + "y": 0.10071294754743576, + "z": -0.013231426477432251 + }, + "angular": { + "x": -0.004493355751037598, + "y": -0.0637570470571518, + "z": -0.24708376824855804 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.13531771302223206, + "y": -0.2937188148498535, + "z": -0.013231366872787476 + }, + "angular": { + "x": 0.04977677762508392, + "y": 0.040092699229717255, + "z": -0.24708381295204163 + } + }, + "gripper_open_percent": 7.809245586395264, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.052665017545223236, + "y": -0.0012662224471569061, + "z": -0.008352491073310375 + }, + "angular": { + "x": 0.05689035728573799, + "y": -0.02616347372531891, + "z": -0.564189612865448 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0367652028799057, + "y": -0.03772934526205063, + "z": -0.008352532051503658 + }, + "angular": { + "x": -0.019540909677743912, + "y": 0.05949113517999649, + "z": -0.564189612865448 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.21607235074043274, + "y": 0.2784590721130371, + "z": -0.0013047456741333008 + }, + "angular": { + "x": -0.03165850043296814, + "y": 0.040404416620731354, + "z": -0.36054569482803345 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.05701325088739395, + "y": -0.3478160500526428, + "z": -0.0013047456741333008 + }, + "angular": { + "x": -0.008064061403274536, + "y": -0.05069267749786377, + "z": -0.36054563522338867 + } + }, + "gripper_open_percent": 7.798564434051514, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0213201604783535, + "y": 0.0033505717292428017, + "z": 0.0019964496605098248 + }, + "angular": { + "x": -0.08654476702213287, + "y": -0.05413699895143509, + "z": -0.5589055418968201 + } + }, + "body_state_vision": { + "linear": { + "x": 0.012052735313773155, + "y": -0.01790228858590126, + "z": 0.0019964089151471853 + }, + "angular": { + "x": 0.09856174886226654, + "y": -0.026578135788440704, + "z": -0.5589055418968201 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.09796179831027985, + "y": 0.36224472522735596, + "z": -0.002080976963043213 + }, + "angular": { + "x": -0.22992867231369019, + "y": -0.1256030797958374, + "z": -0.46448424458503723 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.19878031313419342, + "y": -0.3182826340198517, + "z": -0.0020810365676879883 + }, + "angular": { + "x": 0.24849945306777954, + "y": -0.08301414549350739, + "z": -0.4644842743873596 + } + }, + "gripper_open_percent": 7.788640022277832, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.017620373517274857, + "y": -0.004938716534525156, + "z": -0.01860431209206581 + }, + "angular": { + "x": -0.0013556554913520813, + "y": 0.05706705152988434, + "z": -0.5336633324623108 + } + }, + "body_state_vision": { + "linear": { + "x": -0.008371477946639061, + "y": 0.01627267524600029, + "z": -0.018604284152388573 + }, + "angular": { + "x": -0.040894389152526855, + "y": -0.039826296269893646, + "z": -0.5336633324623108 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.06973684579133987, + "y": 0.40816783905029297, + "z": -0.009777843952178955 + }, + "angular": { + "x": 0.007500588893890381, + "y": -0.022167518734931946, + "z": -0.5174858570098877 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.34654688835144043, + "y": -0.2266482263803482, + "z": -0.009777873754501343 + }, + "angular": { + "x": 0.01113969087600708, + "y": 0.020580731332302094, + "z": -0.5174858570098877 + } + }, + "gripper_open_percent": 7.777190208435059, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.02681594341993332, + "y": -0.03416426479816437, + "z": -0.0168276596814394 + }, + "angular": { + "x": 0.046335428953170776, + "y": -0.055971551686525345, + "z": -0.5013383030891418 + } + }, + "body_state_vision": { + "linear": { + "x": 0.006787203252315521, + "y": 0.042898084968328476, + "z": -0.016827672719955444 + }, + "angular": { + "x": 0.009483888745307922, + "y": 0.07204058021306992, + "z": -0.5013383030891418 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.2236318588256836, + "y": 0.3311997354030609, + "z": 0.0045011937618255615 + }, + "angular": { + "x": 0.10205909609794617, + "y": -0.0016531497240066528, + "z": -0.5710124969482422 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.3948689103126526, + "y": -0.06150399148464203, + "z": 0.0045011937618255615 + }, + "angular": { + "x": -0.06823766231536865, + "y": 0.0759105384349823, + "z": -0.5710126161575317 + } + }, + "gripper_open_percent": 7.76802921295166, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.010476060211658478, + "y": -0.05519535019993782, + "z": -0.0036212909035384655 + }, + "angular": { + "x": 0.023167183622717857, + "y": 0.03366299718618393, + "z": 0.0002570702345110476 + } + }, + "body_state_vision": { + "linear": { + "x": 0.04757416248321533, + "y": 0.029882775619626045, + "z": -0.0036212955601513386 + }, + "angular": { + "x": -0.04043196514248848, + "y": -0.005930796265602112, + "z": 0.00025707026361487806 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.019205499440431595, + "y": -0.04563596472144127, + "z": 0.03624340891838074 + }, + "angular": { + "x": 0.05054257810115814, + "y": 0.048488713800907135, + "z": -0.054730869829654694 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.02037166804075241, + "y": 0.0451274998486042, + "z": 0.03624343499541283 + }, + "angular": { + "x": -0.06992414593696594, + "y": 0.0040404312312603, + "z": -0.05473088473081589 + } + }, + "gripper_open_percent": 7.764214515686035, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.027308039367198944, + "y": 0.1408398151397705, + "z": 0.026218906044960022 + }, + "angular": { + "x": 0.1454344093799591, + "y": -0.2475818544626236, + "z": 0.07549717277288437 + } + }, + "body_state_vision": { + "linear": { + "x": -0.12178541719913483, + "y": -0.07582835853099823, + "z": 0.02621818147599697 + }, + "angular": { + "x": 0.08245494961738586, + "y": 0.2750442326068878, + "z": 0.07549717277288437 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.041849955916404724, + "y": 0.21611125767230988, + "z": 0.016014009714126587 + }, + "angular": { + "x": 0.38963228464126587, + "y": -0.33426007628440857, + "z": -0.22884099185466766 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.1868373304605484, + "y": -0.11639313399791718, + "z": 0.016013264656066895 + }, + "angular": { + "x": -0.02020149677991867, + "y": 0.5129672288894653, + "z": -0.22884097695350647 + } + }, + "gripper_open_percent": 7.748943328857422, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.17415449023246765, + "y": -0.04542165994644165, + "z": 0.03772285580635071 + }, + "angular": { + "x": 0.037927910685539246, + "y": -0.07669901847839355, + "z": 0.030715178698301315 + } + }, + "body_state_vision": { + "linear": { + "x": 0.15179196000099182, + "y": -0.09670630097389221, + "z": 0.03772280737757683 + }, + "angular": { + "x": 0.030393414199352264, + "y": 0.07998441904783249, + "z": 0.030715178698301315 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.2577645182609558, + "y": -0.050034716725349426, + "z": 0.026480555534362793 + }, + "angular": { + "x": -0.0027766674757003784, + "y": 0.21293246746063232, + "z": -0.08307883143424988 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.212067112326622, + "y": -0.15483394265174866, + "z": 0.026480503380298615 + }, + "angular": { + "x": -0.15414059162139893, + "y": -0.14693064987659454, + "z": -0.08307884633541107 + } + }, + "gripper_open_percent": 7.729095458984375, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0018417912069708109, + "y": 0.005220577120780945, + "z": -0.010011238045990467 + }, + "angular": { + "x": -0.03172553703188896, + "y": 0.015394340269267559, + "z": 0.001207878696732223 + } + }, + "body_state_vision": { + "linear": { + "x": -0.005078769288957119, + "y": -0.0022028875537216663, + "z": -0.010011245496571064 + }, + "angular": { + "x": 0.01030808687210083, + "y": -0.03372296690940857, + "z": 0.0012078783474862576 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0017551630735397339, + "y": -0.02461063116788864, + "z": 0.03434671089053154 + }, + "angular": { + "x": 0.012036453932523727, + "y": 0.12280061841011047, + "z": 0.07551374286413193 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.016839534044265747, + "y": 0.018033135682344437, + "z": 0.03434669226408005 + }, + "angular": { + "x": -0.09817485511302948, + "y": -0.07474330067634583, + "z": 0.07551373541355133 + } + }, + "gripper_open_percent": 7.725286483764648, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.33192676305770874, + "y": 0.43388932943344116, + "z": -0.041226889938116074 + }, + "angular": { + "x": 0.057425979524850845, + "y": -0.01856634020805359, + "z": 0.030733592808246613 + } + }, + "body_state_vision": { + "linear": { + "x": -0.09207087755203247, + "y": -0.5384777188301086, + "z": -0.04122689366340637 + }, + "angular": { + "x": -0.025472356006503105, + "y": 0.054713912308216095, + "z": 0.030733592808246613 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.21208205819129944, + "y": 0.2903457283973694, + "z": -0.025656476616859436 + }, + "angular": { + "x": -0.023964786902070045, + "y": -0.03183267265558243, + "z": 0.004099905490875244 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.06843851506710052, + "y": -0.35298120975494385, + "z": -0.02565653622150421 + }, + "angular": { + "x": 0.03963346779346466, + "y": 0.004100814461708069, + "z": 0.0040999166667461395 + } + }, + "gripper_open_percent": 7.504677772521973, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.07012604922056198, + "y": 0.05236998200416565, + "z": -0.008313094265758991 + }, + "angular": { + "x": 0.00559442862868309, + "y": -0.028049245476722717, + "z": -0.006962019484490156 + } + }, + "body_state_vision": { + "linear": { + "x": 0.009344272315502167, + "y": -0.0870228111743927, + "z": -0.008313145488500595 + }, + "angular": { + "x": 0.016746699810028076, + "y": 0.023186352103948593, + "z": -0.006962017621845007 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.15258048474788666, + "y": 0.10607313364744186, + "z": 0.010890822857618332 + }, + "angular": { + "x": 0.0709812343120575, + "y": 0.01843317598104477, + "z": 0.03220631927251816 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.026100805029273033, + "y": -0.1839866042137146, + "z": 0.01089075580239296 + }, + "angular": { + "x": -0.061808452010154724, + "y": 0.0394694060087204, + "z": 0.032206322997808456 + } + }, + "gripper_open_percent": 7.481777667999268, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.01744186505675316, + "y": 0.10402854532003403, + "z": 0.01972019486129284 + }, + "angular": { + "x": 0.022747650742530823, + "y": 0.14295533299446106, + "z": 0.03363191336393356 + } + }, + "body_state_vision": { + "linear": { + "x": -0.08809761703014374, + "y": -0.0580083504319191, + "z": 0.019720079377293587 + }, + "angular": { + "x": -0.12023232877254486, + "y": -0.08060930669307709, + "z": 0.03363191708922386 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.052727557718753815, + "y": 0.19319774210453033, + "z": 0.005079701542854309 + }, + "angular": { + "x": -0.1416248083114624, + "y": -0.02392938733100891, + "z": -0.0427568182349205 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.10568904876708984, + "y": -0.17010389268398285, + "z": 0.005079597234725952 + }, + "angular": { + "x": 0.11390740424394608, + "y": -0.08749450743198395, + "z": -0.0427568256855011 + } + }, + "gripper_open_percent": 7.459646224975586, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.3045295178890228, + "y": 0.18147681653499603, + "z": -0.020634755492210388 + }, + "angular": { + "x": 0.009719735011458397, + "y": 0.051648110151290894, + "z": -0.017772117629647255 + } + }, + "body_state_vision": { + "linear": { + "x": 0.07424569129943848, + "y": -0.3466408848762512, + "z": -0.020634684711694717 + }, + "angular": { + "x": -0.04446016997098923, + "y": -0.028023162856698036, + "z": -0.017772117629647255 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.11592131108045578, + "y": 0.04911596328020096, + "z": 0.014358126558363438 + }, + "angular": { + "x": -0.026624202728271484, + "y": 0.08273033797740936, + "z": -0.0655837431550026 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.042891524732112885, + "y": -0.1183660477399826, + "z": 0.014358185231685638 + }, + "angular": { + "x": -0.04250495135784149, + "y": -0.0758056566119194, + "z": -0.06558378785848618 + } + }, + "gripper_open_percent": 7.4573516845703125, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.05083613842725754, + "y": 0.08527678996324539, + "z": 0.012129811570048332 + }, + "angular": { + "x": -0.09196732193231583, + "y": -0.0021238476037979126, + "z": 0.03723614290356636 + } + }, + "body_state_vision": { + "linear": { + "x": -0.027895167469978333, + "y": -0.09528012573719025, + "z": 0.012129804119467735 + }, + "angular": { + "x": 0.06413812190294266, + "y": -0.06594544649124146, + "z": 0.03723614290356636 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0310354083776474, + "y": 0.03950207680463791, + "z": -0.015327559784054756 + }, + "angular": { + "x": -0.012967891991138458, + "y": -0.006462715566158295, + "z": 0.11167129874229431 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.00782693549990654, + "y": -0.0496220737695694, + "z": -0.015327587723731995 + }, + "angular": { + "x": 0.013560056686401367, + "y": -0.005104731768369675, + "z": 0.1116713136434555 + } + }, + "gripper_open_percent": 7.450490951538086, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.01901763305068016, + "y": -0.00017968658357858658, + "z": 0.01213306188583374 + }, + "angular": { + "x": 0.021591613069176674, + "y": -0.005155512131750584, + "z": 0.0230304803699255 + } + }, + "body_state_vision": { + "linear": { + "x": 0.013072776608169079, + "y": -0.01381324976682663, + "z": 0.012133069336414337 + }, + "angular": { + "x": -0.010914848186075687, + "y": 0.0193298552185297, + "z": 0.0230304803699255 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.03985393047332764, + "y": -0.0041741058230400085, + "z": -0.01999111846089363 + }, + "angular": { + "x": -0.026959478855133057, + "y": -0.08815288543701172, + "z": -0.09537194669246674 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.02406109869480133, + "y": 0.03204406052827835, + "z": -0.019991103559732437 + }, + "angular": { + "x": 0.08294093608856201, + "y": 0.04023115336894989, + "z": -0.09537194669246674 + } + }, + "gripper_open_percent": 7.447427749633789, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.05682288855314255, + "y": -0.04737561196088791, + "z": -0.003414349164813757 + }, + "angular": { + "x": 0.001229338813573122, + "y": 0.01130436360836029, + "z": 0.024499094113707542 + } + }, + "body_state_vision": { + "linear": { + "x": -0.003951460123062134, + "y": 0.07387608289718628, + "z": -0.0034143521916121244 + }, + "angular": { + "x": -0.009120014496147633, + "y": -0.0067915599793195724, + "z": 0.024499094113707542 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.019952574744820595, + "y": -0.03694343939423561, + "z": 0.0013238824903964996 + }, + "angular": { + "x": -0.008482221513986588, + "y": -0.0007399134337902069, + "z": 0.06612221896648407 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.04064825549721718, + "y": 0.01051862258464098, + "z": 0.0013238787651062012 + }, + "angular": { + "x": 0.006314165890216827, + "y": -0.005712013691663742, + "z": 0.06612221151590347 + } + }, + "gripper_open_percent": 7.4451446533203125, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0362163670361042, + "y": 0.02791416645050049, + "z": -0.004477230831980705 + }, + "angular": { + "x": -0.08067592978477478, + "y": 0.035971131175756454, + "z": -0.005134300794452429 + } + }, + "body_state_vision": { + "linear": { + "x": 0.004189860075712204, + "y": -0.04553316533565521, + "z": -0.004477349575608969 + }, + "angular": { + "x": 0.02853976935148239, + "y": -0.08359427750110626, + "z": -0.005134299863129854 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.05617932602763176, + "y": 0.07164980471134186, + "z": -0.03207896649837494 + }, + "angular": { + "x": -0.121781125664711, + "y": -0.013561218976974487, + "z": 0.014069970697164536 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.014273833483457565, + "y": -0.08992253243923187, + "z": -0.03207909315824509 + }, + "angular": { + "x": 0.09280668199062347, + "y": -0.08000916242599487, + "z": 0.01406995952129364 + } + }, + "gripper_open_percent": 7.4390411376953125, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0004544027033261955, + "y": 0.00017560768174007535, + "z": -0.00041092693572863936 + }, + "angular": { + "x": 0.0010259593836963177, + "y": -0.003299734788015485, + "z": -0.0022735637612640858 + } + }, + "body_state_vision": { + "linear": { + "x": -0.00043789230403490365, + "y": 0.00021347386064007878, + "z": -0.0004109287983737886 + }, + "angular": { + "x": 0.0017197956331074238, + "y": 0.002997190225869417, + "z": -0.0022735637612640858 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0005572959780693054, + "y": 0.008646294474601746, + "z": -0.002449851483106613 + }, + "angular": { + "x": 0.004035657271742821, + "y": 0.03394247964024544, + "z": -0.0185238067060709 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.00595649890601635, + "y": -0.006291991099715233, + "z": -0.0024498524144291878 + }, + "angular": { + "x": -0.02761813998222351, + "y": -0.020139941945672035, + "z": -0.01852380484342575 + } + }, + "gripper_open_percent": 11.199945449829102, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.00031486916122958064, + "y": 0.00010293952072970569, + "z": 0.00031964207300916314 + }, + "angular": { + "x": 0.000828900549095124, + "y": 0.0014307977398857474, + "z": -0.0022866842336952686 + } + }, + "body_state_vision": { + "linear": { + "x": 0.000138830320793204, + "y": -0.00030077373958192766, + "z": 0.00031964280060492456 + }, + "angular": { + "x": -0.0016124919056892395, + "y": -0.00036623619962483644, + "z": -0.0022866842336952686 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.002072560368105769, + "y": 0.0006649655988439918, + "z": 0.0005347724654711783 + }, + "angular": { + "x": 0.0030096848495304585, + "y": 0.008476315066218376, + "z": -0.00666704960167408 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0018975997809320688, + "y": 0.0010662112617865205, + "z": 0.0005347731057554483 + }, + "angular": { + "x": -0.008259199559688568, + "y": -0.0035625509917736053, + "z": -0.006667053326964378 + } + }, + "gripper_open_percent": 99.03744506835938, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.00027050820062868297, + "y": 0.0006535720312967896, + "z": 0.0004151844186708331 + }, + "angular": { + "x": -0.00012375600636005402, + "y": 0.0020580855198204517, + "z": -0.006348030176013708 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0006629925919696689, + "y": -0.00024652050342410803, + "z": 0.0004151853499934077 + }, + "angular": { + "x": -0.001423884998075664, + "y": -0.0014911683974787593, + "z": -0.006348030176013708 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.006395743228495121, + "y": 0.0005067992024123669, + "z": 0.005044401623308659 + }, + "angular": { + "x": -0.02808678150177002, + "y": 0.013565568253397942, + "z": -0.01698007993400097 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.004723533522337675, + "y": 0.004341728985309601, + "z": 0.005044402088969946 + }, + "angular": { + "x": 0.009172048419713974, + "y": -0.029812170192599297, + "z": -0.01698007807135582 + } + }, + "gripper_open_percent": 92.17819213867188, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.00018225368694402277, + "y": -5.101779606775381e-05, + "z": -0.0002610658702906221 + }, + "angular": { + "x": -3.0990049708634615e-05, + "y": -0.0008665687637403607, + "z": 0.0004940925282426178 + } + }, + "body_state_vision": { + "linear": { + "x": -8.663560583954677e-05, + "y": 0.00016826583305373788, + "z": -0.00026106645236723125 + }, + "angular": { + "x": 0.0006560820038430393, + "y": 0.000566973234526813, + "z": 0.0004940925282426178 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.002583790337666869, + "y": 0.00040803058072924614, + "z": -0.003583531128242612 + }, + "angular": { + "x": -0.008705406449735165, + "y": -0.012066122144460678, + "z": -0.0029375627636909485 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0020572063513100147, + "y": 0.0016156621277332306, + "z": -0.0035835327580571175 + }, + "angular": { + "x": 0.014765511266887188, + "y": 0.0018316982313990593, + "z": -0.002937561832368374 + } + }, + "gripper_open_percent": 2.463674545288086, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.05577339231967926, + "y": 0.03916638717055321, + "z": 0.006186512298882008 + }, + "angular": { + "x": -0.01947801187634468, + "y": 0.021028803661465645, + "z": -0.07243841886520386 + } + }, + "body_state_vision": { + "linear": { + "x": 0.009252678602933884, + "y": -0.06752081215381622, + "z": 0.006186524871736765 + }, + "angular": { + "x": -0.0021548550575971603, + "y": -0.02858251892030239, + "z": -0.07243841886520386 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.32287824153900146, + "y": -0.2726355791091919, + "z": 0.0843593180179596 + }, + "angular": { + "x": 1.1650961637496948, + "y": 1.6128495931625366, + "z": 2.0872859954833984 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.019933223724365234, + "y": 0.4221176207065582, + "z": 0.08435937762260437 + }, + "angular": { + "x": -1.9746671915054321, + "y": -0.24376505613327026, + "z": 2.0872862339019775 + } + }, + "gripper_open_percent": 2.685040235519409, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.011003841646015644, + "y": 0.006556096486747265, + "z": 0.007706779055297375 + }, + "angular": { + "x": 0.0053788539953529835, + "y": 0.006828385405242443, + "z": -0.0034083768259733915 + } + }, + "body_state_vision": { + "linear": { + "x": 0.002683781087398529, + "y": -0.012524541467428207, + "z": 0.007706786505877972 + }, + "angular": { + "x": -0.008663815446197987, + "y": -0.0007051220163702965, + "z": -0.0034083768259733915 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.1281537115573883, + "y": 0.05357380211353302, + "z": -0.32883960008621216 + }, + "angular": { + "x": 0.7605428695678711, + "y": -0.08140885829925537, + "z": -0.7388010025024414 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.12646296620368958, + "y": 0.05745112895965576, + "z": -0.32883960008621216 + }, + "angular": { + "x": -0.45787906646728516, + "y": 0.6126984357833862, + "z": -0.7388010025024414 + } + }, + "gripper_open_percent": 0.5385696887969971, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.002621863968670368, + "y": 0.002862979657948017, + "z": -0.00576065992936492 + }, + "angular": { + "x": 0.008921841159462929, + "y": -0.005948068108409643, + "z": 0.0002900990657508373 + } + }, + "body_state_vision": { + "linear": { + "x": -0.00031377794221043587, + "y": -0.0038694171234965324, + "z": -0.005760672967880964 + }, + "angular": { + "x": -0.0017125718295574188, + "y": 0.0105851786211133, + "z": 0.00029009912395849824 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0050889234989881516, + "y": 8.765538223087788e-05, + "z": -0.005346089601516724 + }, + "angular": { + "x": 0.01508510485291481, + "y": -0.01823921874165535, + "z": 0.0028100970666855574 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0033986754715442657, + "y": -0.0037886493373662233, + "z": -0.005346103571355343 + }, + "angular": { + "x": 0.003100011497735977, + "y": 0.023465290665626526, + "z": 0.002810097299516201 + } + }, + "gripper_open_percent": 0.5210041999816895, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.018544288352131844, + "y": 0.016651395708322525, + "z": -0.3592769503593445 + }, + "angular": { + "x": -0.0053039733320474625, + "y": 0.013133533298969269, + "z": -0.006808206904679537 + } + }, + "body_state_vision": { + "linear": { + "x": 0.000417390838265419, + "y": -0.024919578805565834, + "z": -0.3592769503593445 + }, + "angular": { + "x": -0.006014588288962841, + "y": -0.012823667377233505, + "z": -0.006808206904679537 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.017470557242631912, + "y": 0.016818612813949585, + "z": -0.36006486415863037 + }, + "angular": { + "x": -0.0016566191334277391, + "y": -0.008777537383139133, + "z": 0.02218693494796753 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.00043578818440437317, + "y": -0.024246571585536003, + "z": -0.36006486415863037 + }, + "angular": { + "x": 0.007559186778962612, + "y": 0.0047590164467692375, + "z": 0.02218693494796753 + } + }, + "gripper_open_percent": 0.5164265632629395, + "stow_state": 1 + } +] \ No newline at end of file diff --git a/spot/spot_data2.json b/spot/spot_data2.json new file mode 100644 index 00000000..19ebb7f7 --- /dev/null +++ b/spot/spot_data2.json @@ -0,0 +1,5991 @@ +[ + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.024382486939430237, + "y": 0.0007379874587059021, + "z": -0.015209006145596504 + }, + "angular": { + "x": 0.06418657302856445, + "y": 0.07949323952198029, + "z": -0.04161308705806732 + } + }, + "body_state_vision": { + "linear": { + "x": 0.016050992533564568, + "y": -0.018368877470493317, + "z": -0.015209002420306206 + }, + "angular": { + "x": -0.10192769765853882, + "y": -0.007059596478939056, + "z": -0.04161308705806732 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.009533199481666088, + "y": -0.01273624412715435, + "z": -0.0015659735072404146 + }, + "angular": { + "x": 0.1276077926158905, + "y": 0.08679305016994476, + "z": -0.0055685159750282764 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.01581985130906105, + "y": 0.0016811322420835495, + "z": -0.0015659660566598177 + }, + "angular": { + "x": -0.15043354034423828, + "y": 0.03444606810808182, + "z": -0.0055685145780444145 + } + }, + "gripper_open_percent": 0.4981040954589844, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.14779938757419586, + "y": -0.1028016209602356, + "z": -0.004406047984957695 + }, + "angular": { + "x": 0.02979366108775139, + "y": 0.0021254681050777435, + "z": -0.01704244501888752 + } + }, + "body_state_vision": { + "linear": { + "x": 0.17590396106243134, + "y": -0.03834836184978485, + "z": -0.0044060382060706615 + }, + "angular": { + "x": -0.021831411868333817, + "y": 0.0203854963183403, + "z": -0.01704244501888752 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.1560773253440857, + "y": -0.10026028752326965, + "z": -0.014071738347411156 + }, + "angular": { + "x": 0.005552052985876799, + "y": -0.027279384434223175, + "z": -0.03591986000537872 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.17967472970485687, + "y": -0.04614351689815521, + "z": -0.014071725308895111 + }, + "angular": { + "x": 0.016211412847042084, + "y": 0.022631395608186722, + "z": -0.03591986000537872 + } + }, + "gripper_open_percent": 0.4904747009277344, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.37528419494628906, + "y": -0.31614768505096436, + "z": -0.015512627549469471 + }, + "angular": { + "x": -0.01032526046037674, + "y": -0.1090652272105217, + "z": -0.020544080063700676 + } + }, + "body_state_vision": { + "linear": { + "x": 0.4870356619358063, + "y": -0.05986447632312775, + "z": -0.015512628480792046 + }, + "angular": { + "x": 0.08694561570882797, + "y": 0.06665051728487015, + "z": -0.020544083788990974 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.40153640508651733, + "y": -0.30766308307647705, + "z": -0.05434398353099823 + }, + "angular": { + "x": -0.07216928899288177, + "y": -0.07160157710313797, + "z": 0.012502976693212986 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.49868255853652954, + "y": -0.0848749577999115, + "z": -0.05434398725628853 + }, + "angular": { + "x": 0.10157699882984161, + "y": -0.004160026088356972, + "z": 0.01250297762453556 + } + }, + "gripper_open_percent": 0.5133748054504395, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.35188978910446167, + "y": -0.30636003613471985, + "z": 0.03274935483932495 + }, + "angular": { + "x": -0.00923408567905426, + "y": -0.016833582893013954, + "z": 0.013335647992789745 + } + }, + "body_state_vision": { + "linear": { + "x": 0.46394452452659607, + "y": -0.04938220977783203, + "z": 0.03274935111403465 + }, + "angular": { + "x": 0.018618715927004814, + "y": 0.004688449203968048, + "z": 0.013335647992789745 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.35420262813568115, + "y": -0.30680105090141296, + "z": 0.025251735001802444 + }, + "angular": { + "x": -0.017522994428873062, + "y": -0.02878662757575512, + "z": 0.0041746883653104305 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.4658415615558624, + "y": -0.050776924937963486, + "z": 0.02525172010064125 + }, + "angular": { + "x": 0.03301795572042465, + "y": 0.00674839410930872, + "z": 0.0041746883653104305 + } + }, + "gripper_open_percent": 0.520247220993042, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.01855522394180298, + "y": 0.017018195241689682, + "z": -0.0045972117222845554 + }, + "angular": { + "x": 0.01623637042939663, + "y": 0.025132257491350174, + "z": -0.00945020280778408 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0001560421660542488, + "y": -0.025177188217639923, + "z": -0.004597194958478212 + }, + "angular": { + "x": -0.02946462295949459, + "y": -0.005204475484788418, + "z": -0.00945020280778408 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.018735138699412346, + "y": 0.01884036883711815, + "z": -0.0026416757609695196 + }, + "angular": { + "x": 0.04698502644896507, + "y": 0.022134285420179367, + "z": -0.023070819675922394 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0010567638091742992, + "y": -0.026548979803919792, + "z": -0.0026416557375341654 + }, + "angular": { + "x": -0.0481916218996048, + "y": 0.019367225468158722, + "z": -0.023070819675922394 + } + }, + "gripper_open_percent": 0.5133748054504395, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.05021650344133377, + "y": -0.024649078026413918, + "z": -0.010749701410531998 + }, + "angular": { + "x": -0.008705422282218933, + "y": -0.034833770245313644, + "z": 0.0020084865391254425 + } + }, + "body_state_vision": { + "linear": { + "x": 0.05223332718014717, + "y": -0.020023880526423454, + "z": -0.010749712586402893 + }, + "angular": { + "x": 0.03144892677664757, + "y": 0.017324578016996384, + "z": 0.0020084865391254425 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.060965005308389664, + "y": -0.01835782825946808, + "z": -0.017442401498556137 + }, + "angular": { + "x": 0.004510741680860519, + "y": -0.005158956162631512, + "z": -0.025957508012652397 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.05493742972612381, + "y": -0.03218110278248787, + "z": -0.01744241453707218 + }, + "angular": { + "x": 0.000710855470970273, + "y": 0.00681588938459754, + "z": -0.025957508012652397 + } + }, + "gripper_open_percent": 0.518721342086792, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.25030067563056946, + "y": -0.271103173494339, + "z": 0.05797644332051277 + }, + "angular": { + "x": -0.10270504653453827, + "y": -0.0109539944678545, + "z": -0.025463860481977463 + } + }, + "body_state_vision": { + "linear": { + "x": 0.36897987127304077, + "y": 0.0010675415396690369, + "z": 0.057976432144641876 + }, + "angular": { + "x": 0.07791530340909958, + "y": -0.06780499964952469, + "z": -0.02546386420726776 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.500602126121521, + "y": -0.20527759194374084, + "z": 0.10372932255268097 + }, + "angular": { + "x": 0.19241328537464142, + "y": -0.6418816447257996, + "z": -0.610962986946106 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.4910697638988495, + "y": -0.22713826596736908, + "z": 0.10372927039861679 + }, + "angular": { + "x": 0.33941715955734253, + "y": 0.5777809023857117, + "z": -0.610962986946106 + } + }, + "gripper_open_percent": 70.92413330078125, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.007586292922496796, + "y": -0.02297133579850197, + "z": -0.006736449431627989 + }, + "angular": { + "x": -0.006411552429199219, + "y": -0.05057110637426376, + "z": -0.09305856376886368 + } + }, + "body_state_vision": { + "linear": { + "x": 0.021994974464178085, + "y": 0.010072601959109306, + "z": -0.0067364792339503765 + }, + "angular": { + "x": 0.04141981899738312, + "y": 0.029714375734329224, + "z": -0.09305856376886368 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.272874653339386, + "y": -0.14325223863124847, + "z": 0.1307075172662735 + }, + "angular": { + "x": 0.5561112761497498, + "y": -1.6292518377304077, + "z": 0.39665818214416504 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.29065608978271484, + "y": -0.102473683655262, + "z": 0.13070747256278992 + }, + "angular": { + "x": 0.8154425024986267, + "y": 1.516171932220459, + "z": 0.3966580927371979 + } + }, + "gripper_open_percent": 97.74054718017578, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.007804295048117638, + "y": -0.005781861022114754, + "z": 0.00011408894351916388 + }, + "angular": { + "x": 0.004379806574434042, + "y": -0.0003223044332116842, + "z": -0.008402416482567787 + } + }, + "body_state_vision": { + "linear": { + "x": 0.009547415189445019, + "y": -0.0017843057867139578, + "z": 0.00011408899445086718 + }, + "angular": { + "x": -0.002744190162047744, + "y": 0.0034287043381482363, + "z": -0.008402416482567787 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.003530755639076233, + "y": 0.004879521206021309, + "z": -0.0050843143835663795 + }, + "angular": { + "x": 0.025267386808991432, + "y": -0.005036097019910812, + "z": 0.018590537831187248 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.005978160537779331, + "y": -0.0007331834640353918, + "z": -0.005084315314888954 + }, + "angular": { + "x": -0.013503624126315117, + "y": 0.02194209210574627, + "z": 0.0185905322432518 + } + }, + "gripper_open_percent": 97.781005859375, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.009906105697154999, + "y": 0.1588556170463562, + "z": 0.004231655970215797 + }, + "angular": { + "x": 0.2000754475593567, + "y": 0.12546904385089874, + "z": 0.09894334524869919 + } + }, + "body_state_vision": { + "linear": { + "x": -0.12314518541097641, + "y": -0.10083891451358795, + "z": 0.004231722094118595 + }, + "angular": { + "x": -0.22808662056922913, + "y": 0.0612298846244812, + "z": 0.09894334524869919 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.29276198148727417, + "y": -0.0948815867304802, + "z": 0.059584274888038635 + }, + "angular": { + "x": 0.21339581906795502, + "y": 0.09238992631435394, + "z": -0.06704317033290863 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.26874443888664246, + "y": -0.14996175467967987, + "z": 0.05958440899848938 + }, + "angular": { + "x": -0.21291157603263855, + "y": 0.09350027143955231, + "z": -0.0670432299375534 + } + }, + "gripper_open_percent": 97.78176879882812, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.1403617262840271, + "y": -0.019597109407186508, + "z": 0.016265161335468292 + }, + "angular": { + "x": -0.004579335451126099, + "y": -0.09079182893037796, + "z": -0.0864555761218071 + } + }, + "body_state_vision": { + "linear": { + "x": 0.10987336933612823, + "y": -0.08951707184314728, + "z": 0.016265057027339935 + }, + "angular": { + "x": 0.06964549422264099, + "y": 0.05842633172869682, + "z": -0.0864555835723877 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.1740112602710724, + "y": -0.000637248158454895, + "z": 0.008138179779052734 + }, + "angular": { + "x": -0.021239042282104492, + "y": -0.1924639344215393, + "z": -0.06945370882749557 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.11887797713279724, + "y": -0.1270761489868164, + "z": 0.00813804566860199 + }, + "angular": { + "x": 0.1554841697216034, + "y": 0.11540433764457703, + "z": -0.06945373862981796 + } + }, + "gripper_open_percent": 97.7894058227539, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.056867025792598724, + "y": -0.0599827766418457, + "z": 0.009251161478459835 + }, + "angular": { + "x": 0.012821834534406662, + "y": 0.05840971693396568, + "z": -0.003664097050204873 + } + }, + "body_state_vision": { + "linear": { + "x": 0.08265026658773422, + "y": -0.0008533112704753876, + "z": 0.009251242503523827 + }, + "angular": { + "x": -0.05152576416730881, + "y": -0.03035116009414196, + "z": -0.0036640965845435858 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.14164617657661438, + "y": -0.1270015388727188, + "z": -0.3618089258670807 + }, + "angular": { + "x": 0.08806639909744263, + "y": 0.14996615052223206, + "z": 0.5937631130218506 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.18944978713989258, + "y": -0.017371902242302895, + "z": -0.3618089556694031 + }, + "angular": { + "x": -0.16981759667396545, + "y": -0.037516601383686066, + "z": 0.5937632322311401 + } + }, + "gripper_open_percent": 97.7985610961914, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.018066657707095146, + "y": 0.07963371276855469, + "z": -0.007945314049720764 + }, + "angular": { + "x": 0.2632133960723877, + "y": -0.09065565466880798, + "z": -0.05969230458140373 + } + }, + "body_state_vision": { + "linear": { + "x": -0.046059075742959976, + "y": -0.06742768734693527, + "z": -0.007945253513753414 + }, + "angular": { + "x": -0.11268148571252823, + "y": 0.254563570022583, + "z": -0.05969231203198433 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.14852184057235718, + "y": -0.1350211203098297, + "z": -0.41534698009490967 + }, + "angular": { + "x": 1.0708913803100586, + "y": 0.39779001474380493, + "z": 2.9491820335388184 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.2000049650669098, + "y": -0.016953006386756897, + "z": -0.4153468608856201 + }, + "angular": { + "x": -1.0202059745788574, + "y": 0.5140267610549927, + "z": 2.9491820335388184 + } + }, + "gripper_open_percent": 97.59246826171875, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.04493315890431404, + "y": -0.0051423050463199615, + "z": -0.010211125947535038 + }, + "angular": { + "x": -0.017203953117132187, + "y": -0.0008028466254472733, + "z": 0.05084574595093727 + } + }, + "body_state_vision": { + "linear": { + "x": 0.03434419259428978, + "y": -0.029426414519548416, + "z": -0.01021112222224474 + }, + "angular": { + "x": 0.012295223772525787, + "y": -0.012060189619660378, + "z": 0.05084574595093727 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0036379098892211914, + "y": -0.011781117878854275, + "z": -0.178850919008255 + }, + "angular": { + "x": 0.020813263952732086, + "y": -0.015783092007040977, + "z": 0.06764562427997589 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.006157338619232178, + "y": 0.010682487860321999, + "z": -0.1788509637117386 + }, + "angular": { + "x": -0.002597622573375702, + "y": 0.025991346687078476, + "z": 0.06764563173055649 + } + }, + "gripper_open_percent": 97.80772399902344, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.003267340362071991, + "y": -0.009175796061754227, + "z": -0.004160336684435606 + }, + "angular": { + "x": -0.03518648445606232, + "y": 0.006285417824983597, + "z": -0.03519575670361519 + } + }, + "body_state_vision": { + "linear": { + "x": 0.004500385373830795, + "y": 0.008638057857751846, + "z": -0.004160366952419281 + }, + "angular": { + "x": 0.019337862730026245, + "y": -0.030060600489377975, + "z": -0.03519575670361519 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.06989720463752747, + "y": -0.05746199190616608, + "z": 0.03221997618675232 + }, + "angular": { + "x": -0.13695143163204193, + "y": 0.1250094473361969, + "z": 0.008550241589546204 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0896698459982872, + "y": -0.012116879224777222, + "z": 0.032219938933849335 + }, + "angular": { + "x": 0.0015895683318376541, + "y": -0.18541984260082245, + "z": 0.008550211787223816 + } + }, + "gripper_open_percent": 49.36779022216797, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0002909693866968155, + "y": 0.020061984658241272, + "z": -0.010880385525524616 + }, + "angular": { + "x": 0.019174203276634216, + "y": -0.002064882777631283, + "z": 0.11600980162620544 + } + }, + "body_state_vision": { + "linear": { + "x": -0.014502723701298237, + "y": -0.0138649120926857, + "z": -0.010880375280976295 + }, + "angular": { + "x": -0.011534547433257103, + "y": 0.015455342829227448, + "z": 0.11600980162620544 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.05569761246442795, + "y": -0.03980976343154907, + "z": -0.022579263895750046 + }, + "angular": { + "x": 0.07003206014633179, + "y": -0.03619552403688431, + "z": 0.08275923132896423 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.008729662746191025, + "y": 0.06790315359830856, + "z": -0.022579241544008255 + }, + "angular": { + "x": -0.021132364869117737, + "y": 0.07594750076532364, + "z": 0.08275923877954483 + } + }, + "gripper_open_percent": 9.528261184692383, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0006732861511409283, + "y": -0.0006589525146409869, + "z": 0.0002152694942196831 + }, + "angular": { + "x": -0.002552502788603306, + "y": 1.4765770174562931e-05, + "z": -0.00431023770943284 + } + }, + "body_state_vision": { + "linear": { + "x": 2.4700129870325327e-05, + "y": 0.0009417672990821302, + "z": 0.00021526854834519327 + }, + "angular": { + "x": 0.0017261039465665817, + "y": -0.0018804403953254223, + "z": -0.00431023770943284 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.007897375151515007, + "y": 0.003414311446249485, + "z": 0.004207686521112919 + }, + "angular": { + "x": -0.016145149245858192, + "y": 0.0036926548928022385, + "z": -0.016688067466020584 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0028720940463244915, + "y": -0.008110306225717068, + "z": 0.004207686521112919 + }, + "angular": { + "x": 0.008280567824840546, + "y": -0.014343418180942535, + "z": -0.016688067466020584 + } + }, + "gripper_open_percent": 9.511465072631836, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.00906267948448658, + "y": 0.001236318377777934, + "z": -0.04902031645178795 + }, + "angular": { + "x": 0.09231976419687271, + "y": -0.16034941375255585, + "z": 0.012827377766370773 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0070728883147239685, + "y": 0.005799556151032448, + "z": -0.04902031645178795 + }, + "angular": { + "x": 0.05467728152871132, + "y": 0.17676328122615814, + "z": 0.012827377766370773 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.12551742792129517, + "y": 0.047350458800792694, + "z": 0.43472403287887573 + }, + "angular": { + "x": 0.010359562933444977, + "y": -0.4041886627674103, + "z": -0.024808049201965332 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.12010884284973145, + "y": 0.05975423753261566, + "z": 0.43472403287887573 + }, + "angular": { + "x": 0.28912705183029175, + "y": 0.28263288736343384, + "z": -0.024808049201965332 + } + }, + "gripper_open_percent": 9.526735305786133, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0012883441522717476, + "y": 0.00333552248775959, + "z": 0.008560714311897755 + }, + "angular": { + "x": -0.0017645814223214984, + "y": 0.010327417403459549, + "z": -0.0026212972588837147 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0015674730530008674, + "y": -0.003213809337466955, + "z": 0.008560704998672009 + }, + "angular": { + "x": -0.006366838701069355, + "y": -0.008320609107613564, + "z": -0.0026212972588837147 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0006094574928283691, + "y": -0.0059494334273040295, + "z": 0.16692595183849335 + }, + "angular": { + "x": -0.018411634489893913, + "y": 0.024731718003749847, + "z": -0.00970412977039814 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0047742873430252075, + "y": 0.0036018728278577328, + "z": 0.16692593693733215 + }, + "angular": { + "x": -0.005593892186880112, + "y": -0.030320854857563972, + "z": -0.00970412790775299 + } + }, + "gripper_open_percent": 9.52291488647461, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0007626971928402781, + "y": 0.0007367244688794017, + "z": 0.0005064998404122889 + }, + "angular": { + "x": -0.003830248024314642, + "y": -0.0011615138500928879, + "z": -0.002564159920439124 + } + }, + "body_state_vision": { + "linear": { + "x": -2.08487908821553e-05, + "y": -0.0010602007387205958, + "z": 0.0005065000732429326 + }, + "angular": { + "x": 0.003457523649558425, + "y": -0.0020162989385426044, + "z": -0.002564159920439124 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.016514547169208527, + "y": 0.016569269821047783, + "z": 0.034107550978660583 + }, + "angular": { + "x": 0.07609742879867554, + "y": -0.09955985844135284, + "z": -0.036798544228076935 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.023379232734441757, + "y": 0.0008263159543275833, + "z": 0.034107550978660583 + }, + "angular": { + "x": 0.02117162011563778, + "y": 0.12351013720035553, + "z": -0.03679855912923813 + } + }, + "gripper_open_percent": 9.52291488647461, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 8.578179404139519e-06, + "y": 0.00032116082729771733, + "z": -0.00021652624127455056 + }, + "angular": { + "x": -5.8898935094475746e-05, + "y": -0.0012489365180954337, + "z": -0.001895447727292776 + } + }, + "body_state_vision": { + "linear": { + "x": -0.00024117289285641164, + "y": -0.00021225929958745837, + "z": -0.00021652414579875767 + }, + "angular": { + "x": 0.0009552594274282455, + "y": 0.0008067145245149732, + "z": -0.001895447727292776 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.011582658626139164, + "y": 0.0032464126124978065, + "z": 0.004223071504384279 + }, + "angular": { + "x": 0.02651088684797287, + "y": -0.04509970545768738, + "z": -0.003252144902944565 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.01026061736047268, + "y": 0.006278281100094318, + "z": 0.004223073832690716 + }, + "angular": { + "x": 0.015007536858320236, + "y": 0.050115689635276794, + "z": -0.0032521411776542664 + } + }, + "gripper_open_percent": 9.523683547973633, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.04315754026174545, + "y": -0.03721809759736061, + "z": -0.00395595608279109 + }, + "angular": { + "x": -0.10604313015937805, + "y": -0.10121937096118927, + "z": -0.0011880823876708746 + } + }, + "body_state_vision": { + "linear": { + "x": 0.056640028953552246, + "y": -0.00629834458231926, + "z": -0.003956038039177656 + }, + "angular": { + "x": 0.14633041620254517, + "y": -0.008827384561300278, + "z": -0.0011880822712555528 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.04976710304617882, + "y": 0.04622192680835724, + "z": 0.03683460131287575 + }, + "angular": { + "x": -0.1554684191942215, + "y": -0.0903741866350174, + "z": -0.0893082246184349 + } + }, + "arm_state_vision": { + "linear": { + "x": -4.492700099945068e-06, + "y": -0.06792077422142029, + "z": 0.03683450445532799 + }, + "angular": { + "x": 0.17201629281044006, + "y": -0.05242452025413513, + "z": -0.0893082469701767 + } + }, + "gripper_open_percent": 9.526735305786133, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.3951459527015686, + "y": -0.3162420392036438, + "z": 0.016465872526168823 + }, + "angular": { + "x": 0.13456465303897858, + "y": 0.038759876042604446, + "z": 0.0465642549097538 + } + }, + "body_state_vision": { + "linear": { + "x": 0.500620424747467, + "y": -0.07435442507266998, + "z": 0.01646590232849121 + }, + "angular": { + "x": -0.11997036635875702, + "y": 0.07222941517829895, + "z": 0.0465642549097538 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.5048611760139465, + "y": -0.22874101996421814, + "z": -0.006014719605445862 + }, + "angular": { + "x": 0.4452664852142334, + "y": 0.18418371677398682, + "z": -0.24760812520980835 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.5111614465713501, + "y": -0.2142927050590515, + "z": -0.006014615297317505 + }, + "angular": { + "x": -0.43795832991600037, + "y": 0.20094388723373413, + "z": -0.24760818481445312 + } + }, + "gripper_open_percent": 9.514516830444336, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.05686362832784653, + "y": 0.019447341561317444, + "z": -0.018073618412017822 + }, + "angular": { + "x": -0.15967629849910736, + "y": 0.007237637415528297, + "z": -0.7896468639373779 + } + }, + "body_state_vision": { + "linear": { + "x": 0.024443965405225754, + "y": -0.0549016036093235, + "z": -0.01807367242872715 + }, + "angular": { + "x": 0.10335274040699005, + "y": -0.12193085253238678, + "z": -0.7896468639373779 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.11637559533119202, + "y": 0.6387006044387817, + "z": -0.0024212002754211426 + }, + "angular": { + "x": -0.196031391620636, + "y": -0.036582253873348236, + "z": -0.720348060131073 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.3888286054134369, + "y": -0.5198983550071716, + "z": -0.002421259880065918 + }, + "angular": { + "x": 0.16020143032073975, + "y": -0.11875225603580475, + "z": -0.7203479409217834 + } + }, + "gripper_open_percent": 9.51681137084961, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.08971694856882095, + "y": 0.020483888685703278, + "z": 0.01441926322877407 + }, + "angular": { + "x": 0.052895620465278625, + "y": -0.09166108071804047, + "z": 0.03247781842947006 + } + }, + "body_state_vision": { + "linear": { + "x": 0.046040501445531845, + "y": -0.07968052476644516, + "z": 0.01441920641809702 + }, + "angular": { + "x": 0.03117198497056961, + "y": 0.10113359987735748, + "z": 0.032477814704179764 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.011479735374450684, + "y": 0.07328447699546814, + "z": -0.00659627839922905 + }, + "angular": { + "x": 0.08993440121412277, + "y": -0.219196155667305, + "z": -0.24347591400146484 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.06151224672794342, + "y": -0.04145648702979088, + "z": -0.006596330553293228 + }, + "angular": { + "x": 0.09942152351140976, + "y": 0.2150593250989914, + "z": -0.24347592890262604 + } + }, + "gripper_open_percent": 9.51223373413086, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.5529217720031738, + "y": 0.29664257168769836, + "z": 0.04688841104507446 + }, + "angular": { + "x": -0.008926652371883392, + "y": 0.0873657837510109, + "z": -0.04848601669073105 + } + }, + "body_state_vision": { + "linear": { + "x": 0.1588813066482544, + "y": -0.6070225238800049, + "z": 0.046888500452041626 + }, + "angular": { + "x": -0.05794445797801018, + "y": -0.06599173694849014, + "z": -0.04848602041602135 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.656724750995636, + "y": 0.3862709403038025, + "z": 0.01800099015235901 + }, + "angular": { + "x": 0.15870429575443268, + "y": 0.006252668797969818, + "z": 0.12910670042037964 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.16384032368659973, + "y": -0.7440760731697083, + "z": 0.018001064658164978 + }, + "angular": { + "x": -0.11257664114236832, + "y": 0.11203857511281967, + "z": 0.12910673022270203 + } + }, + "gripper_open_percent": 9.51375961303711, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.5372318029403687, + "y": 0.2611039876937866, + "z": 0.01590951532125473 + }, + "angular": { + "x": 0.08314254879951477, + "y": -0.02143051102757454, + "z": -0.21735897660255432 + } + }, + "body_state_vision": { + "linear": { + "x": 0.17424604296684265, + "y": -0.5713422298431396, + "z": 0.01590944081544876 + }, + "angular": { + "x": -0.04087308794260025, + "y": 0.07550720870494843, + "z": -0.21735897660255432 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.4652521312236786, + "y": 0.4507235288619995, + "z": -0.017649516463279724 + }, + "angular": { + "x": 0.28551244735717773, + "y": -0.21468406915664673, + "z": -0.13968878984451294 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.013681821525096893, + "y": -0.6476298570632935, + "z": -0.017649691551923752 + }, + "angular": { + "x": -0.03697124123573303, + "y": 0.3553023934364319, + "z": -0.13968876004219055 + } + }, + "gripper_open_percent": 9.51070785522461, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.558289647102356, + "y": 0.37351328134536743, + "z": -0.011240205727517605 + }, + "angular": { + "x": -0.04161236807703972, + "y": -0.08850927650928497, + "z": -0.014829767867922783 + } + }, + "body_state_vision": { + "linear": { + "x": 0.10620510578155518, + "y": -0.6632643342018127, + "z": -0.01124022901058197 + }, + "angular": { + "x": 0.0931730791926384, + "y": 0.029736407101154327, + "z": -0.014829766936600208 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.4952012002468109, + "y": 0.4237150549888611, + "z": -0.010518118739128113 + }, + "angular": { + "x": 0.38321956992149353, + "y": -0.21051672101020813, + "z": 0.09673179686069489 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.026488741859793663, + "y": -0.6511964797973633, + "z": -0.010518074035644531 + }, + "angular": { + "x": -0.1065126582980156, + "y": 0.42406323552131653, + "z": 0.0967317670583725 + } + }, + "gripper_open_percent": 9.507644653320312, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.6338967084884644, + "y": 0.36453837156295776, + "z": -0.06366661190986633 + }, + "angular": { + "x": -0.03752100095152855, + "y": -0.04782482981681824, + "z": -0.04551457241177559 + } + }, + "body_state_vision": { + "linear": { + "x": 0.16423028707504272, + "y": -0.7125594615936279, + "z": -0.06366652250289917 + }, + "angular": { + "x": 0.06057671085000038, + "y": 0.005049530416727066, + "z": -0.04551457241177559 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.509640097618103, + "y": 0.33784937858581543, + "z": -0.010512575507164001 + }, + "angular": { + "x": -0.011913623660802841, + "y": 0.015119247138500214, + "z": -0.03186485543847084 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.09923353791236877, + "y": -0.6033468246459961, + "z": -0.010512501001358032 + }, + "angular": { + "x": -0.0029719769954681396, + "y": -0.019018234685063362, + "z": -0.03186485171318054 + } + }, + "gripper_open_percent": 9.506118774414062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.4622817635536194, + "y": 0.22560368478298187, + "z": -0.01957067847251892 + }, + "angular": { + "x": -0.03658151999115944, + "y": 0.03162040561437607, + "z": -0.008543099276721478 + } + }, + "body_state_vision": { + "linear": { + "x": 0.14925730228424072, + "y": -0.4922642111778259, + "z": -0.01957060769200325 + }, + "angular": { + "x": 0.001722477376461029, + "y": -0.048322807997465134, + "z": -0.008543098345398903 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.5299038290977478, + "y": 0.27601689100265503, + "z": -0.01767146587371826 + }, + "angular": { + "x": -0.053318336606025696, + "y": 0.057702451944351196, + "z": 0.018499337136745453 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.15833152830600739, + "y": -0.5761204957962036, + "z": -0.01767146587371826 + }, + "angular": { + "x": -0.00600059237331152, + "y": -0.07833527773618698, + "z": 0.0184993427246809 + } + }, + "gripper_open_percent": 9.506118774414062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.5321053266525269, + "y": 0.3688865005970001, + "z": -0.05952218174934387 + }, + "angular": { + "x": -0.031515877693891525, + "y": -0.028928250074386597, + "z": 0.040338970720767975 + } + }, + "body_state_vision": { + "linear": { + "x": 0.09177684783935547, + "y": -0.6409294009208679, + "z": -0.05952223390340805 + }, + "angular": { + "x": 0.04264356940984726, + "y": -0.003408856689929962, + "z": 0.040338970720767975 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.45670512318611145, + "y": 0.208965003490448, + "z": -0.02312833070755005 + }, + "angular": { + "x": -0.031950026750564575, + "y": -0.049719274044036865, + "z": 0.19114501774311066 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.15765437483787537, + "y": -0.4768555760383606, + "z": -0.023128420114517212 + }, + "angular": { + "x": 0.058173999190330505, + "y": 0.010420835576951504, + "z": 0.19114501774311066 + } + }, + "gripper_open_percent": 9.50918197631836, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.380694717168808, + "y": 0.2582448720932007, + "z": 0.02836461551487446 + }, + "angular": { + "x": -0.017662987112998962, + "y": -0.0052649034187197685, + "z": -0.03353069722652435 + } + }, + "body_state_vision": { + "linear": { + "x": 0.06981867551803589, + "y": -0.4546915590763092, + "z": 0.028364619240164757 + }, + "angular": { + "x": 0.015877192839980125, + "y": -0.009360309690237045, + "z": -0.03353070095181465 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.4428771734237671, + "y": 0.27441197633743286, + "z": -0.006622642278671265 + }, + "angular": { + "x": 0.07742702960968018, + "y": -0.09408941864967346, + "z": 0.09565097093582153 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.1002853661775589, + "y": -0.5112584829330444, + "z": -0.006622686982154846 + }, + "angular": { + "x": 0.01625896245241165, + "y": 0.12076183408498764, + "z": 0.09565096348524094 + } + }, + "gripper_open_percent": 9.507644653320312, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.42879658937454224, + "y": 0.2633419632911682, + "z": -0.01723654568195343 + }, + "angular": { + "x": -0.010490033775568008, + "y": -0.008803600445389748, + "z": 0.027399098500609398 + } + }, + "body_state_vision": { + "linear": { + "x": 0.09881418943405151, + "y": -0.4934079647064209, + "z": -0.01723656989634037 + }, + "angular": { + "x": 0.013589231297373772, + "y": -0.0016962140798568726, + "z": 0.027399098500609398 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.4550062417984009, + "y": 0.27424541115760803, + "z": -0.020510777831077576 + }, + "angular": { + "x": -0.014274254441261292, + "y": 0.010492706671357155, + "z": -0.03084227256476879 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.10865946114063263, + "y": -0.5200332403182983, + "z": -0.02051083743572235 + }, + "angular": { + "x": 0.002024458721280098, + "y": -0.01759979873895645, + "z": -0.03084227629005909 + } + }, + "gripper_open_percent": 9.506887435913086, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.798450231552124, + "y": 0.38965094089508057, + "z": -0.036557022482156754 + }, + "angular": { + "x": 0.04075644537806511, + "y": 0.05948525667190552, + "z": -0.08947989344596863 + } + }, + "body_state_vision": { + "linear": { + "x": 0.2577889561653137, + "y": -0.8502325415611267, + "z": -0.03655703738331795 + }, + "angular": { + "x": -0.0713229775428772, + "y": -0.0106121264398098, + "z": -0.08947989344596863 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.6395527124404907, + "y": 0.36876052618026733, + "z": -0.04726859927177429 + }, + "angular": { + "x": 0.07334881275892258, + "y": -0.07726088166236877, + "z": -0.0856650173664093 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.1649729311466217, + "y": -0.7195804715156555, + "z": -0.04726865887641907 + }, + "angular": { + "x": 0.006703950464725494, + "y": 0.10632190108299255, + "z": -0.0856650248169899 + } + }, + "gripper_open_percent": 9.506118774414062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.4397343397140503, + "y": 0.2801552414894104, + "z": 0.015759248286485672 + }, + "angular": { + "x": 0.03499597683548927, + "y": 0.04740495979785919, + "z": -0.23810328543186188 + } + }, + "body_state_vision": { + "linear": { + "x": 0.09392854571342468, + "y": -0.5128656625747681, + "z": 0.015759127214550972 + }, + "angular": { + "x": -0.05855100601911545, + "y": -0.006612665951251984, + "z": -0.23810328543186188 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.4554702043533325, + "y": 0.5094029307365417, + "z": 0.0013419613242149353 + }, + "angular": { + "x": 0.05286358296871185, + "y": 0.07484306395053864, + "z": -0.25280749797821045 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.06335321068763733, + "y": -0.6803907752037048, + "z": 0.0013418495655059814 + }, + "angular": { + "x": -0.09081539511680603, + "y": -0.012190112844109535, + "z": -0.25280749797821045 + } + }, + "gripper_open_percent": 9.504592895507812, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.5957593321800232, + "y": 0.4660874903202057, + "z": -0.06787638366222382 + }, + "angular": { + "x": -0.01415177807211876, + "y": 0.013089592568576336, + "z": 0.01789150945842266 + } + }, + "body_state_vision": { + "linear": { + "x": 0.06384700536727905, + "y": -0.753718376159668, + "z": -0.06787638366222382 + }, + "angular": { + "x": 3.7822872400283813e-05, + "y": -0.01927717588841915, + "z": 0.01789150945842266 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.5190064907073975, + "y": 0.3598476052284241, + "z": -0.0132199227809906 + }, + "angular": { + "x": -0.04831792414188385, + "y": -0.01336611993610859, + "z": 0.05933019518852234 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.08947151899337769, + "y": -0.6251827478408813, + "z": -0.013220041990280151 + }, + "angular": { + "x": 0.04267292469739914, + "y": -0.026311656460165977, + "z": 0.05933021008968353 + } + }, + "gripper_open_percent": 9.503067016601562, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.48995137214660645, + "y": 0.3834655284881592, + "z": 0.030782248824834824 + }, + "angular": { + "x": -0.09243287146091461, + "y": 0.13279753923416138, + "z": -0.04641076177358627 + } + }, + "body_state_vision": { + "linear": { + "x": 0.05239492654800415, + "y": -0.6199618577957153, + "z": 0.0307820662856102 + }, + "angular": { + "x": -0.034415096044540405, + "y": -0.1580968201160431, + "z": -0.04641077667474747 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.5124748945236206, + "y": 0.3989010453224182, + "z": 0.009658187627792358 + }, + "angular": { + "x": -0.21424362063407898, + "y": 0.1911071240901947, + "z": 0.023928295820951462 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.05641041696071625, + "y": -0.6469700932502747, + "z": 0.009658023715019226 + }, + "angular": { + "x": 0.005743976682424545, + "y": -0.2870352566242218, + "z": 0.02392827719449997 + } + }, + "gripper_open_percent": 9.509939193725586, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.43969660997390747, + "y": 0.3689073622226715, + "z": -0.014327239245176315 + }, + "angular": { + "x": 0.01998075097799301, + "y": -0.04853254556655884, + "z": 0.045517776161432266 + } + }, + "body_state_vision": { + "linear": { + "x": 0.02886444330215454, + "y": -0.5732306241989136, + "z": -0.014327374286949635 + }, + "angular": { + "x": 0.021967977285385132, + "y": 0.047666035592556, + "z": 0.045517776161432266 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.5013977289199829, + "y": 0.3974301218986511, + "z": -0.03337889909744263 + }, + "angular": { + "x": 0.034370724111795425, + "y": -0.021915800869464874, + "z": -0.012569434940814972 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0499483123421669, + "y": -0.6378529071807861, + "z": -0.03337909281253815 + }, + "angular": { + "x": -0.007328210398554802, + "y": 0.040099263191223145, + "z": -0.012569434940814972 + } + }, + "gripper_open_percent": 9.500783920288086, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.7320704460144043, + "y": 0.5001461505889893, + "z": -0.06630068272352219 + }, + "angular": { + "x": 0.005129098892211914, + "y": 0.01667635329067707, + "z": -0.05926438793540001 + } + }, + "body_state_vision": { + "linear": { + "x": 0.13163143396377563, + "y": -0.8767819404602051, + "z": -0.06630071252584457 + }, + "angular": { + "x": -0.01571044884622097, + "y": -0.007588817737996578, + "z": -0.05926438793540001 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.5564039945602417, + "y": 0.4155426025390625, + "z": -0.05863213539123535 + }, + "angular": { + "x": -0.15757355093955994, + "y": -0.0301571823656559, + "z": -0.07345293462276459 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.07409655302762985, + "y": -0.6904860138893127, + "z": -0.05863213539123535 + }, + "angular": { + "x": 0.12931975722312927, + "y": -0.09494879096746445, + "z": -0.073452889919281 + } + }, + "gripper_open_percent": 9.503067016601562, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.794680655002594, + "y": 0.5498849153518677, + "z": -0.06486838310956955 + }, + "angular": { + "x": -0.11373306810855865, + "y": 0.10067342966794968, + "z": -0.05016053467988968 + } + }, + "body_state_vision": { + "linear": { + "x": 0.13778042793273926, + "y": -0.9565081596374512, + "z": -0.06486818939447403 + }, + "angular": { + "x": 0.0036154985427856445, + "y": -0.15184637904167175, + "z": -0.05016053467988968 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.5683204531669617, + "y": 0.514571487903595, + "z": -0.054370805621147156 + }, + "angular": { + "x": -0.2595484256744385, + "y": 0.005006968975067139, + "z": -0.10414493083953857 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.009632868692278862, + "y": -0.7666023373603821, + "z": -0.05437064170837402 + }, + "angular": { + "x": 0.17293916642665863, + "y": -0.19360411167144775, + "z": -0.10414496064186096 + } + }, + "gripper_open_percent": 9.500783920288086, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.03030623495578766, + "y": 0.029587000608444214, + "z": -0.003923754673451185 + }, + "angular": { + "x": -0.013248572126030922, + "y": -0.021649381145834923, + "z": -0.03464164957404137 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0010596849024295807, + "y": -0.0423407219350338, + "z": -0.003923696465790272 + }, + "angular": { + "x": 0.024879561737179756, + "y": 0.005022644996643066, + "z": -0.03464164957404137 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.12957268953323364, + "y": 0.07965824007987976, + "z": -0.0017075985670089722 + }, + "angular": { + "x": -0.015018852427601814, + "y": -0.010290845297276974, + "z": -0.013558972626924515 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.029793424531817436, + "y": -0.14915382862091064, + "z": -0.0017075464129447937 + }, + "angular": { + "x": 0.017760606482625008, + "y": -0.0040034642443060875, + "z": -0.013558976352214813 + } + }, + "gripper_open_percent": 9.500015258789062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.04223354160785675, + "y": -0.02831968106329441, + "z": -0.0006361859850585461 + }, + "angular": { + "x": 0.010301239788532257, + "y": 0.012663958594202995, + "z": 0.039263468235731125 + } + }, + "body_state_vision": { + "linear": { + "x": -0.007985003292560577, + "y": 0.050218366086483, + "z": -0.0006360430852510035 + }, + "angular": { + "x": -0.01628957688808441, + "y": -0.001068374142050743, + "z": 0.039263468235731125 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.08551684021949768, + "y": -0.028070129454135895, + "z": -0.01579327881336212 + }, + "angular": { + "x": -0.02061282843351364, + "y": -0.07656596601009369, + "z": -0.05842314660549164 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.03761981427669525, + "y": 0.0817665234208107, + "z": -0.015793122351169586 + }, + "angular": { + "x": 0.07013338804244995, + "y": 0.036993760615587234, + "z": -0.058423176407814026 + } + }, + "gripper_open_percent": 9.500015258789062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.010927323251962662, + "y": 0.025223003700375557, + "z": -0.0019461960764601827 + }, + "angular": { + "x": 0.028461318463087082, + "y": -0.022796854376792908, + "z": -0.0024067931808531284 + } + }, + "body_state_vision": { + "linear": { + "x": -0.011047981679439545, + "y": -0.02517039328813553, + "z": -0.001946270582266152 + }, + "angular": { + "x": -0.002660796046257019, + "y": 0.036368440836668015, + "z": -0.0024067931808531284 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0036165104247629642, + "y": 0.004747369792312384, + "z": 0.011069162748754025 + }, + "angular": { + "x": 0.03879621624946594, + "y": 0.010151422582566738, + "z": 0.002393808215856552 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0010180426761507988, + "y": -0.0058805025182664394, + "z": 0.011069085448980331 + }, + "angular": { + "x": -0.03383761644363403, + "y": 0.0215223990380764, + "z": 0.0023938119411468506 + } + }, + "gripper_open_percent": 9.501541137695312, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.10656742751598358, + "y": 0.0185200497508049, + "z": -0.01614176481962204 + }, + "angular": { + "x": 0.19256535172462463, + "y": -0.15645307302474976, + "z": -0.2464808225631714 + } + }, + "body_state_vision": { + "linear": { + "x": -0.08608513325452805, + "y": 0.06549109518527985, + "z": -0.01614128239452839 + }, + "angular": { + "x": -0.016381144523620605, + "y": 0.2475695163011551, + "z": -0.24648083746433258 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.019069675356149673, + "y": -0.06821347773075104, + "z": 0.024821974337100983 + }, + "angular": { + "x": -0.2862835228443146, + "y": 0.24248725175857544, + "z": -0.13678742945194244 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.037010498344898224, + "y": 0.06039002537727356, + "z": 0.024822518229484558 + }, + "angular": { + "x": 0.017105169594287872, + "y": -0.3747873604297638, + "z": -0.13678747415542603 + } + }, + "gripper_open_percent": 9.504592895507812, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.01835402101278305, + "y": -0.09051128476858139, + "z": -0.011073791421949863 + }, + "angular": { + "x": 0.0073072887025773525, + "y": -0.007923027500510216, + "z": -0.6353448033332825 + } + }, + "body_state_vision": { + "linear": { + "x": 0.053836897015571594, + "y": 0.07503947615623474, + "z": -0.011073747649788857 + }, + "angular": { + "x": 0.000833787489682436, + "y": 0.010745962150394917, + "z": -0.6353448033332825 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.4519534111022949, + "y": 0.25643622875213623, + "z": -0.011791020631790161 + }, + "angular": { + "x": -0.043555617332458496, + "y": -0.0810943990945816, + "z": -0.8305988311767578 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.4954463243484497, + "y": 0.15670286118984222, + "z": -0.01179090142250061 + }, + "angular": { + "x": 0.08906280994415283, + "y": 0.02326275408267975, + "z": -0.8305988311767578 + } + }, + "gripper_open_percent": 9.496206283569336, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.005197644233703613, + "y": -0.08037962764501572, + "z": -0.006025152746587992 + }, + "angular": { + "x": 0.00486648827791214, + "y": -0.09801085293292999, + "z": -0.6243024468421936 + } + }, + "body_state_vision": { + "linear": { + "x": 0.06243889033794403, + "y": 0.05088363215327263, + "z": -0.006025162059813738 + }, + "angular": { + "x": 0.0685109794139862, + "y": 0.0702570378780365, + "z": -0.6243024468421936 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.5098516941070557, + "y": 0.03561963140964508, + "z": -0.017378509044647217 + }, + "angular": { + "x": -0.025724351406097412, + "y": -0.09489884972572327, + "z": -0.7924277782440186 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.373027503490448, + "y": 0.3493809103965759, + "z": -0.01737847924232483 + }, + "angular": { + "x": 0.08704614639282227, + "y": 0.04572246968746185, + "z": -0.7924278378486633 + } + }, + "gripper_open_percent": 9.496963500976562, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0988285019993782, + "y": -0.06517455726861954, + "z": -0.010938078165054321 + }, + "angular": { + "x": -0.0028858408331871033, + "y": 0.06124052032828331, + "z": -0.6075422763824463 + } + }, + "body_state_vision": { + "linear": { + "x": 0.11500705778598785, + "y": -0.028073474764823914, + "z": -0.010938138701021671 + }, + "angular": { + "x": -0.042913421988487244, + "y": -0.043785471469163895, + "z": -0.6075422763824463 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.4456747770309448, + "y": -0.21206563711166382, + "z": -0.01377570629119873 + }, + "angular": { + "x": 0.08055377006530762, + "y": 0.03420043736696243, + "z": -0.7303733825683594 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.1478550285100937, + "y": 0.4708898067474365, + "z": -0.013775765895843506 + }, + "angular": { + "x": -0.07987445592880249, + "y": 0.03575829789042473, + "z": -0.7303733825683594 + } + }, + "gripper_open_percent": 9.500783920288086, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.017721140757203102, + "y": 0.017484234645962715, + "z": -0.002955168951302767 + }, + "angular": { + "x": -0.06545153260231018, + "y": 0.014060068875551224, + "z": 0.038503482937812805 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0007543410174548626, + "y": -0.024883221834897995, + "z": -0.002955171512439847 + }, + "angular": { + "x": 0.03423289954662323, + "y": -0.0575300008058548, + "z": 0.038503482937812805 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.04686801880598068, + "y": -0.009217564016580582, + "z": 0.006945515051484108 + }, + "angular": { + "x": -0.09098584204912186, + "y": -0.11759501695632935, + "z": 0.01758674532175064 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.038645558059215546, + "y": -0.02807297185063362, + "z": 0.006945503409951925 + }, + "angular": { + "x": 0.14808443188667297, + "y": 0.013342387974262238, + "z": 0.017586752772331238 + } + }, + "gripper_open_percent": 9.494668006896973, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.02864440530538559, + "y": 0.10279468446969986, + "z": 0.00907517597079277 + }, + "angular": { + "x": -0.0024932846426963806, + "y": 0.015455568209290504, + "z": 0.03465685248374939 + } + }, + "body_state_vision": { + "linear": { + "x": -0.09481924772262573, + "y": -0.0489557608962059, + "z": 0.009075300768017769 + }, + "angular": { + "x": -0.009629311040043831, + "y": -0.012343749403953552, + "z": 0.03465684875845909 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.027209527790546417, + "y": 0.04280748963356018, + "z": -0.007224589586257935 + }, + "angular": { + "x": -0.062162745743989944, + "y": 0.04937557131052017, + "z": 0.034441202878952026 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.012855086475610733, + "y": -0.04906757175922394, + "z": -0.007224489003419876 + }, + "angular": { + "x": 0.006115864962339401, + "y": -0.07915021479129791, + "z": 0.034441180527210236 + } + }, + "gripper_open_percent": 9.496963500976562, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.23405663669109344, + "y": 0.2508358955383301, + "z": -0.01161055639386177 + }, + "angular": { + "x": 0.06849093735218048, + "y": 0.023003965616226196, + "z": 0.03421281278133392 + } + }, + "body_state_vision": { + "linear": { + "x": -0.34307530522346497, + "y": 0.0008368119597434998, + "z": -0.011610528454184532 + }, + "angular": { + "x": -0.06346158683300018, + "y": 0.034537263214588165, + "z": 0.03421281278133392 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.1614876091480255, + "y": 0.2563317120075226, + "z": 0.012976497411727905 + }, + "angular": { + "x": 0.1208607405424118, + "y": 0.04053877294063568, + "z": 0.17377641797065735 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.29772335290908813, + "y": -0.05608132481575012, + "z": 0.01297653466463089 + }, + "angular": { + "x": -0.11194578558206558, + "y": 0.06098242104053497, + "z": 0.17377644777297974 + } + }, + "gripper_open_percent": 9.496963500976562, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.1694737672805786, + "y": 0.27800068259239197, + "z": -0.015563174150884151 + }, + "angular": { + "x": -0.06509941816329956, + "y": -0.0027907714247703552, + "z": -0.0005584174650721252 + } + }, + "body_state_vision": { + "linear": { + "x": -0.3190363943576813, + "y": -0.06497327983379364, + "z": -0.015563268214464188 + }, + "angular": { + "x": 0.04634156823158264, + "y": -0.0458059124648571, + "z": -0.0005584153113886714 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.17609214782714844, + "y": 0.3322903513908386, + "z": -0.0165584534406662 + }, + "angular": { + "x": 0.016369912773370743, + "y": 0.005076650530099869, + "z": 0.007904228754341602 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.3633233308792114, + "y": -0.09706437587738037, + "z": -0.01655857264995575 + }, + "angular": { + "x": -0.014858964830636978, + "y": 0.008541513234376907, + "z": 0.007904227823019028 + } + }, + "gripper_open_percent": 9.497732162475586, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.08366366475820541, + "y": 0.059925638139247894, + "z": -0.010796857997775078 + }, + "angular": { + "x": -0.03749658912420273, + "y": 0.05491313338279724, + "z": -0.003943013027310371 + } + }, + "body_state_vision": { + "linear": { + "x": -0.1008419618010521, + "y": 0.02053278312087059, + "z": -0.01079676765948534 + }, + "angular": { + "x": -0.014726076275110245, + "y": -0.0648428425192833, + "z": -0.003943013958632946 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.1286713033914566, + "y": 0.1970260888338089, + "z": -0.03603840619325638 + }, + "angular": { + "x": -0.10665315389633179, + "y": 0.11554819345474243, + "z": -0.01079973578453064 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.23193441331386566, + "y": -0.03977470099925995, + "z": -0.03603833168745041 + }, + "angular": { + "x": -0.012102354317903519, + "y": -0.15677952766418457, + "z": -0.010799750685691833 + } + }, + "gripper_open_percent": 9.495437622070312, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.020405594259500504, + "y": 0.07111229002475739, + "z": 0.0027709982823580503 + }, + "angular": { + "x": 0.0022199759259819984, + "y": -0.020368868485093117, + "z": -0.020475251600146294 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0659959614276886, + "y": -0.03343462571501732, + "z": 0.002770879538729787 + }, + "angular": { + "x": 0.013415761291980743, + "y": 0.015486683696508408, + "z": -0.020475251600146294 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.005720821674913168, + "y": 0.019380025565624237, + "z": -0.003249702975153923 + }, + "angular": { + "x": 0.029422402381896973, + "y": -0.019954929128289223, + "z": 0.01102178543806076 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.010308939963579178, + "y": -0.01737917959690094, + "z": -0.0032498184591531754 + }, + "angular": { + "x": -0.005397300235927105, + "y": 0.035138972103595734, + "z": 0.01102178543806076 + } + }, + "gripper_open_percent": 9.496206283569336, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.011415358632802963, + "y": 0.05970105901360512, + "z": -0.021953538060188293 + }, + "angular": { + "x": -0.1517626792192459, + "y": 0.053616613149642944, + "z": 0.10187248140573502 + } + }, + "body_state_vision": { + "linear": { + "x": -0.051516927778720856, + "y": -0.032258424907922745, + "z": -0.021953849121928215 + }, + "angular": { + "x": 0.06397569179534912, + "y": -0.14769497513771057, + "z": 0.10187248140573502 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.14546993374824524, + "y": -0.17303884029388428, + "z": 0.02237623929977417 + }, + "angular": { + "x": 3.259575366973877, + "y": 0.2997264862060547, + "z": -1.5454193353652954 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.22578658163547516, + "y": 0.011142179369926453, + "z": 0.02237590402364731 + }, + "angular": { + "x": -2.437596082687378, + "y": 2.184673547744751, + "z": -1.5454193353652954 + } + }, + "gripper_open_percent": 9.507644653320312, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0025917927268892527, + "y": 0.0017547851894050837, + "z": -0.0032745797652751207 + }, + "angular": { + "x": -0.0015776515938341618, + "y": -0.005633937194943428, + "z": -0.004644349217414856 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0004776735440827906, + "y": -0.003093285020440817, + "z": -0.00327460584230721 + }, + "angular": { + "x": 0.005202058237046003, + "y": 0.0026774737052619457, + "z": -0.004644349217414856 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0014913731720298529, + "y": -0.0005513534415513277, + "z": 0.006091406103223562 + }, + "angular": { + "x": 0.1046411320567131, + "y": 0.10127702355384827, + "z": -0.01095567550510168 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0014188411878421903, + "y": -0.000717700575478375, + "z": 0.006091380957514048 + }, + "angular": { + "x": -0.1454182267189026, + "y": 0.0077678244560956955, + "z": -0.01095567550510168 + } + }, + "gripper_open_percent": 9.488564491271973, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.00013134448090568185, + "y": 0.0009755657520145178, + "z": 0.17826977372169495 + }, + "angular": { + "x": 0.062302954494953156, + "y": -0.04860368371009827, + "z": 0.0014665734488517046 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0008042900590226054, + "y": -0.0005675810971297324, + "z": 0.17826959490776062 + }, + "angular": { + "x": -0.006776854395866394, + "y": 0.0787278413772583, + "z": 0.0014665735652670264 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.011299416422843933, + "y": -0.0053126029670238495, + "z": 0.03340085968375206 + }, + "angular": { + "x": 0.04725676402449608, + "y": -0.03369438648223877, + "z": 0.012111233547329903 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.011581715196371078, + "y": -0.004665296524763107, + "z": 0.033400729298591614 + }, + "angular": { + "x": -0.0074643222615122795, + "y": 0.05755702033638954, + "z": 0.012111234478652477 + } + }, + "gripper_open_percent": 9.490090370178223, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0008199305739253759, + "y": 0.00486144982278347, + "z": 0.00195480533875525 + }, + "angular": { + "x": -0.012082571163773537, + "y": 0.008759529329836369, + "z": -0.0030386182479560375 + } + }, + "body_state_vision": { + "linear": { + "x": -0.004120395518839359, + "y": -0.0027071000076830387, + "z": 0.0019548095297068357 + }, + "angular": { + "x": 0.0018025137251242995, + "y": -0.014814469963312149, + "z": -0.0030386182479560375 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.017593933269381523, + "y": -0.02295961044728756, + "z": -0.012738627381622791 + }, + "angular": { + "x": -0.1541943997144699, + "y": 0.10693925619125366, + "z": 0.00042756661423482 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.02879648655653, + "y": 0.002729865489527583, + "z": -0.012738621793687344 + }, + "angular": { + "x": 0.02655540592968464, + "y": -0.18575985729694366, + "z": 0.00042756643961183727 + } + }, + "gripper_open_percent": 9.490090370178223, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.002358468249440193, + "y": 0.004849082790315151, + "z": 0.00020577458781190217 + }, + "angular": { + "x": -0.005333945620805025, + "y": -0.001322794472798705, + "z": -0.0028291649650782347 + } + }, + "body_state_vision": { + "linear": { + "x": -0.005158206447958946, + "y": -0.0015712252352386713, + "z": 0.00020574958762153983 + }, + "angular": { + "x": 0.004598797764629126, + "y": -0.0030086347833275795, + "z": -0.0028291649650782347 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.03265148401260376, + "y": -0.045698381960392, + "z": -0.005687651690095663 + }, + "angular": { + "x": -0.1263883262872696, + "y": 0.10966441035270691, + "z": -0.0038824714720249176 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.055705323815345764, + "y": 0.007168145384639502, + "z": -0.00568767823278904 + }, + "angular": { + "x": 0.005637935362756252, + "y": -0.16723792254924774, + "z": -0.003882471937686205 + } + }, + "gripper_open_percent": 9.489334106445312, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0014432122698053718, + "y": -0.0028081052005290985, + "z": -0.002801693044602871 + }, + "angular": { + "x": 0.009615452028810978, + "y": -0.005884464364498854, + "z": -0.0016482578357681632 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0030398271046578884, + "y": 0.0008531815838068724, + "z": -0.0028016872238367796 + }, + "angular": { + "x": -0.002230636077001691, + "y": 0.011050266213715076, + "z": -0.0016482577193528414 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.062376342713832855, + "y": 0.0859447717666626, + "z": -0.028505636379122734 + }, + "angular": { + "x": 0.28904882073402405, + "y": -0.15261144936084747, + "z": -0.0008598723215982318 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.10542398691177368, + "y": -0.012771103531122208, + "z": -0.028505628928542137 + }, + "angular": { + "x": -0.0848475992679596, + "y": 0.315658837556839, + "z": -0.0008598732529208064 + } + }, + "gripper_open_percent": 9.487808227539062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.00023588433396071196, + "y": 0.0010595563799142838, + "z": -0.00029219099087640643 + }, + "angular": { + "x": 0.00031860655872151256, + "y": -0.00060678506270051, + "z": -0.002850428456440568 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0009369543986395001, + "y": -0.0005481153493747115, + "z": -0.00029218828422017395 + }, + "angular": { + "x": 0.00022785860346630216, + "y": 0.0006463577738031745, + "z": -0.002850428456440568 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0017850808799266815, + "y": 0.0026164380833506584, + "z": 0.13596957921981812 + }, + "angular": { + "x": 0.05303168296813965, + "y": -0.008089214563369751, + "z": 0.0001943745301105082 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.003131979377940297, + "y": -0.00047223397996276617, + "z": 0.13596957921981812 + }, + "angular": { + "x": -0.03015727363526821, + "y": 0.04436589404940605, + "z": 0.00019437470473349094 + } + }, + "gripper_open_percent": 9.487808227539062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.00030186964431777596, + "y": -0.0009383972501382232, + "z": 0.0017832742305472493 + }, + "angular": { + "x": 0.0019164038822054863, + "y": -0.0028555728495121002, + "z": -0.0023991300258785486 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0008930551121011376, + "y": 0.00041730637894943357, + "z": 0.0017832766752690077 + }, + "angular": { + "x": 0.0007885610684752464, + "y": 0.003347395220771432, + "z": -0.0023991300258785486 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.03349411487579346, + "y": 0.047641318291425705, + "z": -0.03390275314450264 + }, + "angular": { + "x": -0.11962355673313141, + "y": 0.10975701361894608, + "z": 0.0007494965684600174 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0577024407684803, + "y": -0.00787272583693266, + "z": -0.03390275314450264 + }, + "angular": { + "x": 0.000967070460319519, + "y": -0.16234362125396729, + "z": 0.000749496859498322 + } + }, + "gripper_open_percent": 9.487808227539062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.007614782080054283, + "y": -0.009960453025996685, + "z": -0.00047025742242112756 + }, + "angular": { + "x": -0.007370823994278908, + "y": 0.0020368294790387154, + "z": -0.0002732435241341591 + } + }, + "body_state_vision": { + "linear": { + "x": 0.012480475008487701, + "y": 0.001197413308545947, + "z": -0.00047028594417497516 + }, + "angular": { + "x": 0.003522844985127449, + "y": -0.0067873019725084305, + "z": -0.00027324369875714183 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0745835155248642, + "y": 0.1008872464299202, + "z": 0.002798709087073803 + }, + "angular": { + "x": -0.0040854234248399734, + "y": 0.015918277204036713, + "z": -0.010588360019028187 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.12468017637729645, + "y": -0.01399320550262928, + "z": 0.002798675326630473 + }, + "angular": { + "x": -0.008885023184120655, + "y": -0.013825299218297005, + "z": -0.010588360950350761 + } + }, + "gripper_open_percent": 9.487808227539062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.007664949633181095, + "y": -0.010886349715292454, + "z": -0.00096217478858307 + }, + "angular": { + "x": -0.004189569503068924, + "y": 0.0017126449383795261, + "z": -0.004191892221570015 + } + }, + "body_state_vision": { + "linear": { + "x": 0.013193108141422272, + "y": 0.001790677080862224, + "z": -0.0009621771750971675 + }, + "angular": { + "x": 0.0015957390423864126, + "y": -0.004235480912029743, + "z": -0.004191892221570015 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.07778705656528473, + "y": 0.10871056467294693, + "z": -0.019230032339692116 + }, + "angular": { + "x": -0.009143523871898651, + "y": 0.007607282605022192, + "z": -0.001670629484578967 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.13259288668632507, + "y": -0.016968967393040657, + "z": -0.019230037927627563 + }, + "angular": { + "x": 0.0006470304215326905, + "y": -0.011876711621880531, + "z": -0.001670629484578967 + } + }, + "gripper_open_percent": 9.487038612365723, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.020804638043045998, + "y": 0.019740628078579903, + "z": -0.07211177796125412 + }, + "angular": { + "x": -0.1861235499382019, + "y": 0.16380801796913147, + "z": -0.037719305604696274 + } + }, + "body_state_vision": { + "linear": { + "x": -0.02862226963043213, + "y": 0.0018132776021957397, + "z": -0.07211177796125412 + }, + "angular": { + "x": 0.006607960909605026, + "y": -0.24785363674163818, + "z": -0.037719305604696274 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.2739362418651581, + "y": -0.36532995104789734, + "z": -0.13581442832946777 + }, + "angular": { + "x": -0.9329713582992554, + "y": 0.16897428035736084, + "z": -0.49380871653556824 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.4541122317314148, + "y": 0.04784566909074783, + "z": -0.13581448793411255 + }, + "angular": { + "x": 0.5110097527503967, + "y": -0.79865962266922, + "z": -0.49380871653556824 + } + }, + "gripper_open_percent": 9.486282348632812, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.008491688407957554, + "y": 0.013497251085937023, + "z": 0.010417484678328037 + }, + "angular": { + "x": -0.00503851193934679, + "y": -0.0011452320031821728, + "z": -0.0110405832529068 + } + }, + "body_state_vision": { + "linear": { + "x": -0.01566890999674797, + "y": -0.002961394377052784, + "z": 0.01041747909039259 + }, + "angular": { + "x": 0.004267650656402111, + "y": -0.0029129597824066877, + "z": -0.0110405832529068 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.11280936747789383, + "y": -0.12965163588523865, + "z": -0.7056758999824524 + }, + "angular": { + "x": 0.04935462772846222, + "y": -0.056286849081516266, + "z": 0.12786681950092316 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.1717691868543625, + "y": 0.005553824827075005, + "z": -0.7056758999824524 + }, + "angular": { + "x": 0.007664008066058159, + "y": 0.07446712255477905, + "z": 0.12786681950092316 + } + }, + "gripper_open_percent": 9.487808227539062, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0016772036906331778, + "y": 0.001910294289700687, + "z": -0.008302925154566765 + }, + "angular": { + "x": 0.013175698928534985, + "y": 0.005858954973518848, + "z": -0.0013833676930516958 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0002586092450655997, + "y": -0.002528894692659378, + "z": -0.008302776142954826 + }, + "angular": { + "x": -0.013258790597319603, + "y": 0.005668481811881065, + "z": -0.0013833678094670177 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0015769219025969505, + "y": -0.0037060785107314587, + "z": 0.0017220162553712726 + }, + "angular": { + "x": 0.0008582044392824173, + "y": -0.03439801186323166, + "z": -0.008642998524010181 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0016428260132670403, + "y": 0.0036773630417883396, + "z": 0.0017221680609509349 + }, + "angular": { + "x": 0.02462291717529297, + "y": 0.024034816771745682, + "z": -0.008642998524010181 + } + }, + "gripper_open_percent": 9.917556762695312, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.12591411173343658, + "y": -0.011774882674217224, + "z": -0.02977602742612362 + }, + "angular": { + "x": 0.1587962508201599, + "y": 0.22706389427185059, + "z": -0.0787254348397255 + } + }, + "body_state_vision": { + "linear": { + "x": 0.09430599212646484, + "y": -0.08425772190093994, + "z": -0.029776031151413918 + }, + "angular": { + "x": -0.27444446086883545, + "y": -0.038138095289468765, + "z": -0.0787254348397255 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.06488217413425446, + "y": -0.06681542098522186, + "z": 0.008049193769693375 + }, + "angular": { + "x": 0.3991127610206604, + "y": 0.3115985691547394, + "z": 0.059727203100919724 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.09311097860336304, + "y": -0.002081485465168953, + "z": 0.0080491928383708 + }, + "angular": { + "x": -0.4999133348464966, + "y": 0.08044496178627014, + "z": 0.05972720682621002 + } + }, + "gripper_open_percent": 9.930533409118652, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.11688084900379181, + "y": -0.2791135311126709, + "z": 0.03924507275223732 + }, + "angular": { + "x": -0.18253269791603088, + "y": -0.05244932323694229, + "z": -0.08175082504749298 + } + }, + "body_state_vision": { + "linear": { + "x": 0.28406596183776855, + "y": 0.10427066683769226, + "z": 0.03924453631043434 + }, + "angular": { + "x": 0.16263815760612488, + "y": -0.09807133674621582, + "z": -0.08175082504749298 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.09872190654277802, + "y": -0.274053156375885, + "z": 0.027711518108844757 + }, + "angular": { + "x": 0.08594582974910736, + "y": -0.060605864971876144, + "z": -0.052830491214990616 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.2680015563964844, + "y": 0.11413425952196121, + "z": 0.02771097794175148 + }, + "angular": { + "x": -0.01406916044652462, + "y": 0.10422006249427795, + "z": -0.05283048003911972 + } + }, + "gripper_open_percent": 9.925192832946777, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.16302207112312317, + "y": -0.12167872488498688, + "z": 0.011909504421055317 + }, + "angular": { + "x": -0.024735677987337112, + "y": -0.07283905148506165, + "z": -0.0029911850579082966 + } + }, + "body_state_vision": { + "linear": { + "x": 0.20009353756904602, + "y": -0.03666721284389496, + "z": 0.011909563094377518 + }, + "angular": { + "x": 0.07020772993564606, + "y": 0.031436510384082794, + "z": -0.0029911850579082966 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.1782974898815155, + "y": -0.1180632933974266, + "z": 0.02456538751721382 + }, + "angular": { + "x": -0.014285393059253693, + "y": -0.06624908745288849, + "z": -0.014025319367647171 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.20783819258213043, + "y": -0.05032113194465637, + "z": 0.024565447121858597 + }, + "angular": { + "x": 0.05826776474714279, + "y": 0.03461039066314697, + "z": -0.014025319367647171 + } + }, + "gripper_open_percent": 9.928244590759277, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0007581915706396103, + "y": -0.0046239253133535385, + "z": -0.007740615867078304 + }, + "angular": { + "x": -0.011097896844148636, + "y": 0.012688299641013145, + "z": 0.001516525400802493 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0039043156430125237, + "y": 0.002590713556855917, + "z": -0.0077406009659171104 + }, + "angular": { + "x": -0.0017465041019022465, + "y": -0.016766222193837166, + "z": 0.001516525400802493 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0352473147213459, + "y": 0.043550364673137665, + "z": 0.3335351347923279 + }, + "angular": { + "x": -0.177481546998024, + "y": 0.12883006036281586, + "z": -0.0037255464121699333 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.05589757487177849, + "y": -0.003804305335506797, + "z": 0.3335351347923279 + }, + "angular": { + "x": 0.026359394192695618, + "y": -0.21772025525569916, + "z": -0.0037255471106618643 + } + }, + "gripper_open_percent": 9.922141075134277, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.003220723243430257, + "y": -0.005924338009208441, + "z": 0.004235110245645046 + }, + "angular": { + "x": 0.011545537039637566, + "y": -0.013651030138134956, + "z": 0.007442658767104149 + } + }, + "body_state_vision": { + "linear": { + "x": 0.006532843224704266, + "y": 0.0016709815245121717, + "z": 0.004235072992742062 + }, + "angular": { + "x": 0.0021473984234035015, + "y": 0.017749354243278503, + "z": 0.007442658767104149 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.013030169531702995, + "y": 0.033659450709819794, + "z": 0.6332640647888184 + }, + "angular": { + "x": 0.6814561486244202, + "y": -0.4981885552406311, + "z": 0.09467323124408722 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.01579941436648369, + "y": -0.03245193138718605, + "z": 0.6332640051841736 + }, + "angular": { + "x": -0.09861946105957031, + "y": 0.8383610248565674, + "z": 0.09467323869466782 + } + }, + "gripper_open_percent": 9.934348106384277, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0040439218282699585, + "y": -0.008783438242971897, + "z": 0.27043893933296204 + }, + "angular": { + "x": 0.0778542011976242, + "y": -0.051156919449567795, + "z": 0.005362253170460463 + } + }, + "body_state_vision": { + "linear": { + "x": 0.009188186377286911, + "y": 0.0030132480897009373, + "z": 0.2704389691352844 + }, + "angular": { + "x": -0.015487542375922203, + "y": 0.09186099469661713, + "z": 0.005362252239137888 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.01304963231086731, + "y": -0.01646615006029606, + "z": 0.08000817894935608 + }, + "angular": { + "x": 0.1094004213809967, + "y": -0.03756782412528992, + "z": 0.00785137340426445 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.020945949479937553, + "y": 0.0016415102872997522, + "z": 0.08000820875167847 + }, + "angular": { + "x": -0.04691106081008911, + "y": 0.10573145747184753, + "z": 0.00785137340426445 + } + }, + "gripper_open_percent": 9.9320650100708, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0005929722683504224, + "y": 0.0006132858106866479, + "z": -5.0741724407998845e-05 + }, + "angular": { + "x": -0.0005182640161365271, + "y": 0.0035472395829856396, + "z": -0.0001483544911025092 + } + }, + "body_state_vision": { + "linear": { + "x": -0.000852899276651442, + "y": 1.722466549836099e-05, + "z": -5.071565101388842e-05 + }, + "angular": { + "x": -0.002246771939098835, + "y": -0.0027934834361076355, + "z": -0.00014835446199867874 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.003579834010452032, + "y": -6.350688636302948e-05, + "z": 0.11616410315036774 + }, + "angular": { + "x": -0.013906576670706272, + "y": 0.012842345051467419, + "z": 0.0015690081054344773 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0023893367033451796, + "y": 0.002666519023478031, + "z": 0.11616412550210953 + }, + "angular": { + "x": 5.17603475600481e-05, + "y": -0.018929246813058853, + "z": 0.0015690079890191555 + } + }, + "gripper_open_percent": 9.929770469665527, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.019934462383389473, + "y": -0.02997330203652382, + "z": -0.0032108481973409653 + }, + "angular": { + "x": -0.008213942870497704, + "y": 0.0026149535551667213, + "z": -0.001434803125448525 + } + }, + "body_state_vision": { + "linear": { + "x": 0.035528745502233505, + "y": 0.0057871476747095585, + "z": -0.0032108526211231947 + }, + "angular": { + "x": 0.003672883380204439, + "y": -0.007798513397574425, + "z": -0.001434803125448525 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.21149753034114838, + "y": 0.2983085811138153, + "z": -0.007752187550067902 + }, + "angular": { + "x": 0.027581525966525078, + "y": -0.0454876609146595, + "z": -0.0033576132263988256 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.36251288652420044, + "y": -0.04799649864435196, + "z": -0.007752192206680775 + }, + "angular": { + "x": 0.014565736055374146, + "y": 0.05116354674100876, + "z": -0.0033576132263988256 + } + }, + "gripper_open_percent": 9.929770469665527, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.001921864110045135, + "y": -0.0006892548408359289, + "z": -0.011242386884987354 + }, + "angular": { + "x": -0.02211184613406658, + "y": 0.016686975955963135, + "z": -0.00010752576781669632 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0008026337018236518, + "y": 0.0018773407209664583, + "z": -0.011242431588470936 + }, + "angular": { + "x": 0.002817624481394887, + "y": -0.027558105066418648, + "z": -0.00010752579692052677 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.009591290727257729, + "y": -0.023341191932559013, + "z": 0.09835272282361984 + }, + "angular": { + "x": 0.10686592757701874, + "y": -0.0944969579577446, + "z": -0.0034061463084071875 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.023630768060684204, + "y": 0.008853845298290253, + "z": 0.09835267812013626 + }, + "angular": { + "x": -0.0034687453880906105, + "y": 0.14261122047901154, + "z": -0.0034061467740684748 + } + }, + "gripper_open_percent": 9.931296348571777, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0015546258073300123, + "y": 0.003665351076051593, + "z": 0.000964305829256773 + }, + "angular": { + "x": -0.007076555863022804, + "y": -0.0112239308655262, + "z": 0.016952041536569595 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0016281374264508486, + "y": -0.0036332947202026844, + "z": 0.000964305829256773 + }, + "angular": { + "x": 0.013040101155638695, + "y": 0.0024515436962246895, + "z": 0.016952041536569595 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.13666221499443054, + "y": -0.10408373177051544, + "z": 0.014449255540966988 + }, + "angular": { + "x": 0.047584809362888336, + "y": -0.02625398524105549, + "z": -0.20984044671058655 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.016718236729502678, + "y": 0.17096929252147675, + "z": 0.014449257403612137 + }, + "angular": { + "x": -0.013139823451638222, + "y": 0.05273454636335373, + "z": -0.20984044671058655 + } + }, + "gripper_open_percent": 9.930533409118652, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.027116572484374046, + "y": 0.037408534437417984, + "z": -0.05535973235964775 + }, + "angular": { + "x": -0.09764689952135086, + "y": 0.08892817795276642, + "z": 0.011261926032602787 + } + }, + "body_state_vision": { + "linear": { + "x": -0.04586432874202728, + "y": -0.005583357997238636, + "z": -0.0553593747317791 + }, + "angular": { + "x": 0.0012765554711222649, + "y": -0.13206638395786285, + "z": 0.011261926032602787 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.3021203875541687, + "y": -0.42884114384651184, + "z": -0.48961812257766724 + }, + "angular": { + "x": -2.0348920822143555, + "y": 1.500451683998108, + "z": -0.3042217195034027 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.5198310613632202, + "y": 0.07040805369615555, + "z": -0.48961782455444336 + }, + "angular": { + "x": 0.2850967347621918, + "y": -2.512144088745117, + "z": -0.3042217195034027 + } + }, + "gripper_open_percent": 9.922141075134277, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.033012501895427704, + "y": 0.04149528965353966, + "z": 0.003052558284252882 + }, + "angular": { + "x": 0.0013272333890199661, + "y": -0.011799611151218414, + "z": -0.007950689643621445 + } + }, + "body_state_vision": { + "linear": { + "x": -0.052870918065309525, + "y": -0.004043596796691418, + "z": 0.0030525585170835257 + }, + "angular": { + "x": 0.007743656635284424, + "y": 0.009001562371850014, + "z": -0.007950689643621445 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.3128802180290222, + "y": -0.35516247153282166, + "z": -0.1850816309452057 + }, + "angular": { + "x": 0.5779250264167786, + "y": -0.40781474113464355, + "z": 0.10489733517169952 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.4731605350971222, + "y": 0.012389127165079117, + "z": -0.1850816309452057 + }, + "angular": { + "x": -0.0943983644247055, + "y": 0.7009985446929932, + "z": 0.10489733517169952 + } + }, + "gripper_open_percent": 9.9351167678833, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0004707347834482789, + "y": 0.0007571311434730887, + "z": -0.003318678354844451 + }, + "angular": { + "x": -0.028901882469654083, + "y": 0.025032639503479004, + "z": -0.002537835855036974 + } + }, + "body_state_vision": { + "linear": { + "x": -0.000875109457410872, + "y": -0.00017021343228407204, + "z": -0.0033186038490384817 + }, + "angular": { + "x": 0.00132215884514153, + "y": -0.03821266442537308, + "z": -0.002537835855036974 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.019304567947983742, + "y": 0.010862560011446476, + "z": 0.10213864594697952 + }, + "angular": { + "x": -0.08201249688863754, + "y": 0.055310677736997604, + "z": -0.038777001202106476 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.021095745265483856, + "y": 0.006755059584975243, + "z": 0.1021387055516243 + }, + "angular": { + "x": 0.015273163095116615, + "y": -0.09773465991020203, + "z": -0.038777001202106476 + } + }, + "gripper_open_percent": 9.925192832946777, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0044828299432992935, + "y": 0.006748698651790619, + "z": -0.0016521427314728498 + }, + "angular": { + "x": -0.03431997075676918, + "y": 0.023968849331140518, + "z": -0.0053182062692940235 + } + }, + "body_state_vision": { + "linear": { + "x": -0.00799572467803955, + "y": -0.0013070444110780954, + "z": -0.0016521128127351403 + }, + "angular": { + "x": 0.005788423120975494, + "y": -0.04145921766757965, + "z": -0.0053182062692940235 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.012658276595175266, + "y": -0.004342435859143734, + "z": 0.22593392431735992 + }, + "angular": { + "x": -0.33067435026168823, + "y": 0.15552282333374023, + "z": 0.1417265385389328 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.011795427650213242, + "y": -0.00632118247449398, + "z": 0.22593389451503754 + }, + "angular": { + "x": 0.11103799939155579, + "y": -0.34814316034317017, + "z": 0.1417265385389328 + } + }, + "gripper_open_percent": 9.920608520507812, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.009238695725798607, + "y": -0.008616085164248943, + "z": 0.24303744733333588 + }, + "angular": { + "x": 0.06114464998245239, + "y": -0.028619010001420975, + "z": 0.0021686668042093515 + } + }, + "body_state_vision": { + "linear": { + "x": 0.012600287795066833, + "y": -0.0009073619730770588, + "z": 0.2430373579263687 + }, + "angular": { + "x": -0.020633433014154434, + "y": 0.0642804503440857, + "z": 0.002168667269870639 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.010089671239256859, + "y": -0.007687440142035484, + "z": 0.03086584061384201 + }, + "angular": { + "x": 0.1370507776737213, + "y": -0.032475054264068604, + "z": 0.015331381931900978 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.01249881274998188, + "y": -0.0021628523245453835, + "z": 0.030865751206874847 + }, + "angular": { + "x": -0.0694575309753418, + "y": 0.12252834439277649, + "z": 0.015331384725868702 + } + }, + "gripper_open_percent": 9.931296348571777, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0008239457383751869, + "y": 0.006047429051250219, + "z": -0.0009390994091518223 + }, + "angular": { + "x": -0.006243942305445671, + "y": 0.0009179264307022095, + "z": -0.0015051423106342554 + } + }, + "body_state_vision": { + "linear": { + "x": -0.004992205649614334, + "y": -0.0035111424513161182, + "z": -0.0009391119237989187 + }, + "angular": { + "x": 0.003575995098799467, + "y": -0.005200167186558247, + "z": -0.0015051424270495772 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.0021966728381812572, + "y": 0.009799539111554623, + "z": 0.11858624964952469 + }, + "angular": { + "x": 0.027831409126520157, + "y": -0.017122220247983932, + "z": -0.0011424835538491607 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.008675821125507355, + "y": -0.005058306269347668, + "z": 0.1185862347483635 + }, + "angular": { + "x": -0.0063905189745128155, + "y": 0.03204559162259102, + "z": -0.0011424836702644825 + } + }, + "gripper_open_percent": 9.934348106384277, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.005681912414729595, + "y": -0.010670633055269718, + "z": -0.0028354772366583347 + }, + "angular": { + "x": 0.005234978161752224, + "y": -0.007696299348026514, + "z": -0.00822574831545353 + } + }, + "body_state_vision": { + "linear": { + "x": 0.011685717850923538, + "y": 0.0030970810912549496, + "z": -0.0028354956302791834 + }, + "angular": { + "x": 0.002077748067677021, + "y": 0.009073098190128803, + "z": -0.00822574831545353 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.12161406129598618, + "y": 0.15395155549049377, + "z": -0.03469141945242882 + }, + "angular": { + "x": -0.03221968933939934, + "y": 0.02544548735022545, + "z": -0.021651793271303177 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.1955672800540924, + "y": -0.015636418014764786, + "z": -0.03469143435359001 + }, + "angular": { + "x": 0.003277203068137169, + "y": -0.04092482849955559, + "z": -0.021651793271303177 + } + }, + "gripper_open_percent": 9.929770469665527, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.025167275220155716, + "y": -0.035935983061790466, + "z": 0.004079875536262989 + }, + "angular": { + "x": -0.01563136652112007, + "y": 0.00719990860670805, + "z": 0.0011873984476551414 + } + }, + "body_state_vision": { + "linear": { + "x": 0.04345879703760147, + "y": 0.006009796168655157, + "z": 0.004079875070601702 + }, + "angular": { + "x": 0.005360154435038567, + "y": -0.016353793442249298, + "z": 0.001187398680485785 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.25908827781677246, + "y": 0.37962788343429565, + "z": -0.008876698091626167 + }, + "angular": { + "x": -0.0022086547687649727, + "y": 0.009911859408020973, + "z": 0.004349261987954378 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.45448631048202515, + "y": -0.06845515966415405, + "z": -0.008876693435013294 + }, + "angular": { + "x": -0.00576055608689785, + "y": -0.008362953551113605, + "z": 0.004349261987954378 + } + }, + "gripper_open_percent": 9.93359088897705, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0035616594832390547, + "y": 0.0058356537483632565, + "z": -0.04943438619375229 + }, + "angular": { + "x": 0.015848953276872635, + "y": -0.004738227464258671, + "z": -0.006040018983185291 + } + }, + "body_state_vision": { + "linear": { + "x": -0.00669986754655838, + "y": -0.001360824448056519, + "z": -0.049434367567300797 + }, + "angular": { + "x": -0.007312145084142685, + "y": 0.014838233590126038, + "z": -0.006040018983185291 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.05162464454770088, + "y": -0.07504968345165253, + "z": -0.004606258124113083 + }, + "angular": { + "x": 0.7799416780471802, + "y": -0.5319234132766724, + "z": 0.0004909825511276722 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.09012417495250702, + "y": 0.0132365133613348, + "z": -0.004606232047080994 + }, + "angular": { + "x": -0.14091238379478455, + "y": 0.9334860444068909, + "z": 0.0004909820854663849 + } + }, + "gripper_open_percent": 9.935873985290527, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.002400393830612302, + "y": -0.0011544154258444905, + "z": -0.0001897326874313876 + }, + "angular": { + "x": -0.0008064769208431244, + "y": -0.0033380461391061544, + "z": -0.0011236588470637798 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0007873779395595193, + "y": 0.0025445162318646908, + "z": -0.00018972152611240745 + }, + "angular": { + "x": 0.0029948833398520947, + "y": 0.0016803625039756298, + "z": -0.0011236588470637798 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.005645442754030228, + "y": -0.0002921987324953079, + "z": 0.00646743830293417 + }, + "angular": { + "x": -0.01358442846685648, + "y": -0.016440389677882195, + "z": -0.002319923834875226 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.003627282800152898, + "y": 0.004335802048444748, + "z": 0.006467447616159916 + }, + "angular": { + "x": 0.021290961652994156, + "y": 0.0012320717796683311, + "z": -0.0023199240677058697 + } + }, + "gripper_open_percent": 10.703783988952637, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.03035001829266548, + "y": 0.011564282700419426, + "z": -0.03911777213215828 + }, + "angular": { + "x": -0.14412379264831543, + "y": 0.09903758764266968, + "z": -0.10880857706069946 + } + }, + "body_state_vision": { + "linear": { + "x": -0.029125388711690903, + "y": 0.014372285455465317, + "z": -0.03911752998828888 + }, + "angular": { + "x": 0.025493280962109566, + "y": -0.1730036437511444, + "z": -0.10880857706069946 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.14482572674751282, + "y": -0.3362160325050354, + "z": 0.013349313288927078 + }, + "angular": { + "x": -2.076718330383301, + "y": 0.15707862377166748, + "z": -1.4404817819595337 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.3449256122112274, + "y": 0.12264817953109741, + "z": 0.01334947720170021 + }, + "angular": { + "x": 1.2979824542999268, + "y": -1.6287052631378174, + "z": -1.4404817819595337 + } + }, + "gripper_open_percent": 10.63813591003418, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.002486873883754015, + "y": -0.00033840304240584373, + "z": 0.00785810872912407 + }, + "angular": { + "x": 0.005467407405376434, + "y": 0.0014631939120590687, + "z": 0.002005974994972348 + } + }, + "body_state_vision": { + "linear": { + "x": -0.001444199588149786, + "y": 0.0020526391454041004, + "z": 0.00785813108086586 + }, + "angular": { + "x": -0.004792494233697653, + "y": 0.0030108997598290443, + "z": 0.002005974994972348 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.08178135752677917, + "y": 0.04662860557436943, + "z": -0.46182966232299805 + }, + "angular": { + "x": -0.21325750648975372, + "y": 0.1545957624912262, + "z": 0.5641161203384399 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.021478094160556793, + "y": -0.09165757149457932, + "z": -0.46182963252067566 + }, + "angular": { + "x": 0.03182172775268555, + "y": -0.26146894693374634, + "z": 0.5641161203384399 + } + }, + "gripper_open_percent": 10.618287086486816, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0001764076587278396, + "y": -0.00038096244679763913, + "z": -0.0005343406228348613 + }, + "angular": { + "x": 0.004520857241004705, + "y": 0.003044981975108385, + "z": -0.001076913671568036 + } + }, + "body_state_vision": { + "linear": { + "x": 0.00015913837705738842, + "y": 0.00038849847624078393, + "z": -0.0005343182710930705 + }, + "angular": { + "x": -0.005307555664330721, + "y": 0.001240950427018106, + "z": -0.001076913671568036 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0003748761664610356, + "y": -0.00023640994913876057, + "z": 0.0007953268941491842 + }, + "angular": { + "x": 0.007580382749438286, + "y": 0.014192040078341961, + "z": 0.012310491874814034 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0004283277376089245, + "y": -0.00011384204844944179, + "z": 0.0007953494205139577 + }, + "angular": { + "x": -0.015557968989014626, + "y": -0.004101970233023167, + "z": 0.012310491874814034 + } + }, + "gripper_open_percent": 10.377079010009766, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.003275982104241848, + "y": -0.005022098310291767, + "z": -0.02430308796465397 + }, + "angular": { + "x": -0.02492568828165531, + "y": 0.01111525483429432, + "z": -0.0006175492890179157 + } + }, + "body_state_vision": { + "linear": { + "x": 0.005909317638725042, + "y": 0.0010166120482608676, + "z": -0.02430308796465397 + }, + "angular": { + "x": 0.008815247565507889, + "y": -0.02582886442542076, + "z": -0.0006175492890179157 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.020129632204771042, + "y": 0.03248818963766098, + "z": 0.27087971568107605 + }, + "angular": { + "x": -0.13281035423278809, + "y": 0.10244040936231613, + "z": 0.00048177747521549463 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.037504445761442184, + "y": -0.007355359382927418, + "z": 0.27087971568107605 + }, + "angular": { + "x": 0.015301496721804142, + "y": -0.16702839732170105, + "z": 0.0004817773588001728 + } + }, + "gripper_open_percent": 10.366392135620117, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.005999826826155186, + "y": -0.009686418808996677, + "z": 0.10596960783004761 + }, + "angular": { + "x": 0.11110188066959381, + "y": -0.0655595138669014, + "z": -0.005490477662533522 + } + }, + "body_state_vision": { + "linear": { + "x": 0.011180803179740906, + "y": 0.002194418804720044, + "z": 0.10596960037946701 + }, + "angular": { + "x": -0.027556516230106354, + "y": 0.12602518498897552, + "z": -0.005490477662533522 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.011790676042437553, + "y": 0.014383377507328987, + "z": 0.03632374107837677 + }, + "angular": { + "x": 0.0720716267824173, + "y": -0.15488220751285553, + "z": 0.03902767598628998 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.01856299489736557, + "y": -0.0011468129232525826, + "z": 0.03632378950715065 + }, + "angular": { + "x": 0.06445714831352234, + "y": 0.1582028865814209, + "z": 0.03902767598628998 + } + }, + "gripper_open_percent": 10.370969772338867, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.003239705692976713, + "y": 0.008370993658900261, + "z": 0.009982188232243061 + }, + "angular": { + "x": -0.013642555102705956, + "y": 0.0100173968821764, + "z": -0.0013597403885796666 + } + }, + "body_state_vision": { + "linear": { + "x": -0.008338706567883492, + "y": -0.003321925178170204, + "z": 0.009982221759855747 + }, + "angular": { + "x": 0.0019422369077801704, + "y": -0.01681356690824032, + "z": -0.0013597405049949884 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.011375460773706436, + "y": -0.0008164085447788239, + "z": 0.19509409368038177 + }, + "angular": { + "x": -0.2870730459690094, + "y": 0.26582813262939453, + "z": -0.014997079968452454 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.007142098154872656, + "y": 0.008891468867659569, + "z": 0.19509412348270416 + }, + "angular": { + "x": 0.0005379170179367065, + "y": -0.3912484645843506, + "z": -0.014997082762420177 + } + }, + "gripper_open_percent": 10.369443893432617, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.008446567691862583, + "y": -0.014572549611330032, + "z": 0.0013798577710986137 + }, + "angular": { + "x": 0.0073111290112137794, + "y": -0.010680141858756542, + "z": -0.0059698596596717834 + } + }, + "body_state_vision": { + "linear": { + "x": 0.01642623171210289, + "y": 0.0037261699326336384, + "z": 0.0013798171421512961 + }, + "angular": { + "x": 0.0028516086749732494, + "y": 0.01262484397739172, + "z": -0.0059698596596717834 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.09681731462478638, + "y": 0.12091954052448273, + "z": -0.0018798336386680603 + }, + "angular": { + "x": -0.015336371958255768, + "y": 0.001457315869629383, + "z": -0.00869263056665659 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.15448865294456482, + "y": -0.01133105531334877, + "z": -0.0018798704259097576 + }, + "angular": { + "x": 0.00936761125922203, + "y": -0.012230122461915016, + "z": -0.00869263056665659 + } + }, + "gripper_open_percent": 10.370969772338867, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.02425924688577652, + "y": -0.0321660041809082, + "z": -0.006865051109343767 + }, + "angular": { + "x": -0.002100822050124407, + "y": 0.001904184347949922, + "z": -0.00231630215421319 + } + }, + "body_state_vision": { + "linear": { + "x": 0.04007837548851967, + "y": 0.004109968431293964, + "z": -0.006865048781037331 + }, + "angular": { + "x": 3.410302451811731e-05, + "y": -0.0028351761866360903, + "z": -0.00231630215421319 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.26290151476860046, + "y": 0.3524121940135956, + "z": -0.016932914033532143 + }, + "angular": { + "x": -0.027179056778550148, + "y": -0.017922351136803627, + "z": -0.0020587206818163395 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.4371376633644104, + "y": -0.04714206978678703, + "z": -0.01693292148411274 + }, + "angular": { + "x": 0.03162732720375061, + "y": -0.007721696048974991, + "z": -0.0020587199833244085 + } + }, + "gripper_open_percent": 10.370969772338867, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.019940178841352463, + "y": 0.03410114347934723, + "z": -0.05148901417851448 + }, + "angular": { + "x": -0.11617214977741241, + "y": 0.11419808864593506, + "z": -0.012574232183396816 + } + }, + "body_state_vision": { + "linear": { + "x": -0.038557469844818115, + "y": -0.00859166868031025, + "z": -0.051488831639289856 + }, + "angular": { + "x": -0.004635868594050407, + "y": -0.16283652186393738, + "z": -0.012574232183396816 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.30010372400283813, + "y": -0.3914583623409271, + "z": -0.4239457845687866 + }, + "angular": { + "x": -1.7668014764785767, + "y": 1.1934247016906738, + "z": -0.027818046510219574 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.49106457829475403, + "y": 0.04644905775785446, + "z": -0.42394569516181946 + }, + "angular": { + "x": 0.3276658058166504, + "y": -2.106771230697632, + "z": -0.027818042784929276 + } + }, + "gripper_open_percent": 10.354942321777344, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0016305141616612673, + "y": 0.0017685308121144772, + "z": -0.00017724622739478946 + }, + "angular": { + "x": -0.03124392405152321, + "y": 0.025482214987277985, + "z": -0.003426329232752323 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0024054269306361675, + "y": -8.522707503288984e-06, + "z": -0.00017721642507240176 + }, + "angular": { + "x": 0.0025863514747470617, + "y": -0.040234822779893875, + "z": -0.003426329232752323 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.029090719297528267, + "y": 0.013834025710821152, + "z": -0.6351652145385742 + }, + "angular": { + "x": -0.22846901416778564, + "y": 0.2170538455247879, + "z": 0.18148306012153625 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.009657027199864388, + "y": -0.030730951577425003, + "z": -0.6351652145385742 + }, + "angular": { + "x": -0.0035969391465187073, + "y": -0.31511515378952026, + "z": 0.18148306012153625 + } + }, + "gripper_open_percent": 10.358762741088867, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0006687180139124393, + "y": -0.002158785704523325, + "z": -0.00033828153391368687 + }, + "angular": { + "x": 0.0030479838605970144, + "y": -0.003109078388661146, + "z": -0.0010160678066313267 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0020369826816022396, + "y": 0.000978893251158297, + "z": -0.00033827408333308995 + }, + "angular": { + "x": 0.0002043542917817831, + "y": 0.004349114838987589, + "z": -0.0010160678066313267 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0026665760669857264, + "y": -0.0023670578375458717, + "z": 0.002157442271709442 + }, + "angular": { + "x": 0.005583812948316336, + "y": -0.004827589727938175, + "z": 0.005945029202848673 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0035490321461111307, + "y": -0.0003434204845689237, + "z": 0.0021574492566287518 + }, + "angular": { + "x": -0.00026180839631706476, + "y": 0.007376720197498798, + "z": 0.005945029202848673 + } + }, + "gripper_open_percent": 10.24578857421875, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0027373789343982935, + "y": -0.004273897036910057, + "z": -0.014856128953397274 + }, + "angular": { + "x": -0.005486292764544487, + "y": 0.010388176888227463, + "z": 0.002602157648652792 + } + }, + "body_state_vision": { + "linear": { + "x": 0.004994538612663746, + "y": 0.000902185682207346, + "z": -0.01485609170049429 + }, + "angular": { + "x": -0.0038793538697063923, + "y": -0.011088933795690536, + "z": 0.002602157648652792 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.016161100938916206, + "y": 0.023168835788965225, + "z": 0.21752680838108063 + }, + "angular": { + "x": -0.14391344785690308, + "y": 0.12022320926189423, + "z": 0.0017318936297670007 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.027974873781204224, + "y": -0.003922217525541782, + "z": 0.21752683818340302 + }, + "angular": { + "x": 0.009825262241065502, + "y": -0.18726500868797302, + "z": 0.001731893396936357 + } + }, + "gripper_open_percent": 10.23738956451416, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.006342961452901363, + "y": -0.012253769673407078, + "z": 0.18507884442806244 + }, + "angular": { + "x": 0.09549298137426376, + "y": -0.05755964666604996, + "z": 0.002146238461136818 + } + }, + "body_state_vision": { + "linear": { + "x": 0.013295605778694153, + "y": 0.0036898679099977016, + "z": 0.18507890403270721 + }, + "angular": { + "x": -0.022797834128141403, + "y": 0.10914340615272522, + "z": 0.002146240323781967 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.01741182990372181, + "y": -0.011423842050135136, + "z": 0.1583665907382965 + }, + "angular": { + "x": 0.01110783964395523, + "y": -0.03482726588845253, + "z": -0.03269968554377556 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.003476354293525219, + "y": 0.020532682538032532, + "z": 0.15836665034294128 + }, + "angular": { + "x": 0.017963172867894173, + "y": 0.031837835907936096, + "z": -0.03269968926906586 + } + }, + "gripper_open_percent": 10.24578857421875, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.002863686066120863, + "y": -0.0007195700891315937, + "z": 0.0014719079481437802 + }, + "angular": { + "x": 0.0004628179594874382, + "y": 0.005237814970314503, + "z": -0.0014504323480650783 + } + }, + "body_state_vision": { + "linear": { + "x": -0.001421270426362753, + "y": 0.0025881449691951275, + "z": 0.0014719299506396055 + }, + "angular": { + "x": -0.004153198096901178, + "y": -0.0032248864881694317, + "z": -0.0014504322316497564 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0006524110212922096, + "y": -0.00813551340252161, + "z": 0.05127296969294548 + }, + "angular": { + "x": -0.1075218990445137, + "y": 0.042824096977710724, + "z": 0.00967569462954998 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.006405644118785858, + "y": 0.005057677626609802, + "z": 0.05127299204468727 + }, + "angular": { + "x": 0.041781049221754074, + "y": -0.10793149471282959, + "z": 0.00967569462954998 + } + }, + "gripper_open_percent": 10.24349308013916, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.001150839147157967, + "y": -0.0006625457899644971, + "z": -0.0008877385989762843 + }, + "angular": { + "x": -0.002966436790302396, + "y": 7.159984670579433e-05, + "z": -0.0047753057442605495 + } + }, + "body_state_vision": { + "linear": { + "x": -0.00029755401192232966, + "y": 0.0012941739987581968, + "z": -0.0008877433720044792 + }, + "angular": { + "x": 0.0019660249818116426, + "y": -0.002222527051344514, + "z": -0.0047753057442605495 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0009755268110893667, + "y": -0.001649770187214017, + "z": -0.0036728333216160536 + }, + "angular": { + "x": -0.2025214284658432, + "y": 0.15613526105880737, + "z": 0.015324335545301437 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.0018727562855929136, + "y": 0.0004077213234268129, + "z": -0.003672836348414421 + }, + "angular": { + "x": 0.023388346657156944, + "y": -0.2546490430831909, + "z": 0.015324335545301437 + } + }, + "gripper_open_percent": 10.24273681640625, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.01707746647298336, + "y": -0.028865916654467583, + "z": -0.00011210632510483265 + }, + "angular": { + "x": -0.003674326930195093, + "y": 0.007063143886625767, + "z": -0.012117920443415642 + } + }, + "body_state_vision": { + "linear": { + "x": 0.032773323357105255, + "y": 0.007127329707145691, + "z": -0.00011208117939531803 + }, + "angular": { + "x": -0.0026757060550153255, + "y": -0.007498627062886953, + "z": -0.012117920443415642 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.2313631922006607, + "y": 0.29806816577911377, + "z": -0.04201840981841087 + }, + "angular": { + "x": -0.08581587672233582, + "y": 0.04910646006464958, + "z": -0.009975820779800415 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.37585413455963135, + "y": -0.03327525407075882, + "z": -0.042018383741378784 + }, + "angular": { + "x": 0.02240760251879692, + "y": -0.0963001400232315, + "z": -0.009975820779800415 + } + }, + "gripper_open_percent": 10.23891544342041, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.030678194016218185, + "y": -0.042519256472587585, + "z": -0.04611697420477867 + }, + "angular": { + "x": -0.1057756319642067, + "y": 0.07407213002443314, + "z": -0.005739907268434763 + } + }, + "body_state_vision": { + "linear": { + "x": 0.05203282833099365, + "y": 0.00645090127363801, + "z": -0.046117059886455536 + }, + "angular": { + "x": 0.017694177106022835, + "y": -0.1279142200946808, + "z": -0.005739905871450901 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.24332666397094727, + "y": 0.3268975019454956, + "z": 0.07238741964101791 + }, + "angular": { + "x": 0.14422646164894104, + "y": -0.2253946214914322, + "z": 0.07064534723758698 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.4051206409931183, + "y": -0.04412519559264183, + "z": 0.07238742709159851 + }, + "angular": { + "x": 0.06703140586614609, + "y": 0.25905752182006836, + "z": 0.07064535468816757 + } + }, + "gripper_open_percent": 10.2412109375, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.00018870293570216745, + "y": -0.0001659379922784865, + "z": -8.766450264374726e-06 + }, + "angular": { + "x": 0.0006382183637470007, + "y": -0.003001208184286952, + "z": -0.003129363525658846 + } + }, + "body_state_vision": { + "linear": { + "x": -6.795082299504429e-06, + "y": 0.00025119585916399956, + "z": -8.768298357608728e-06 + }, + "angular": { + "x": 0.0017650160007178783, + "y": 0.0025098398327827454, + "z": -0.003129363525658846 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.002584797330200672, + "y": -0.0023377700708806515, + "z": -0.015476880595088005 + }, + "angular": { + "x": -0.053798675537109375, + "y": 0.03331834077835083, + "z": -0.002594857243821025 + } + }, + "arm_state_vision": { + "linear": { + "x": -4.568113945424557e-05, + "y": 0.00348486565053463, + "z": -0.015476882457733154 + }, + "angular": { + "x": 0.012191252782940865, + "y": -0.062094978988170624, + "z": -0.0025948574766516685 + } + }, + "gripper_open_percent": 10.599207878112793, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -8.20779605419375e-05, + "y": -2.033644705079496e-05, + "z": 0.0002162823366234079 + }, + "angular": { + "x": 0.0015697787748649716, + "y": -0.0011702119372785091, + "z": -0.0015972257824614644 + } + }, + "body_state_vision": { + "linear": { + "x": 7.074653694871813e-05, + "y": -4.63106480310671e-05, + "z": 0.00021628278773277998 + }, + "angular": { + "x": -0.00021061320148874074, + "y": 0.0019465975929051638, + "z": -0.0015972257824614644 + } + }, + "arm_state_odom": { + "linear": { + "x": 0.009707009419798851, + "y": -0.002456073183566332, + "z": -0.013262436725199223 + }, + "angular": { + "x": -0.01276394259184599, + "y": -0.009363893419504166, + "z": -0.010532082989811897 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.004805262200534344, + "y": 0.008784515783190727, + "z": -0.013262436725199223 + }, + "angular": { + "x": 0.015547004528343678, + "y": -0.002981826663017273, + "z": -0.010532082989811897 + } + }, + "gripper_open_percent": 99.71833038330078, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0007896472816355526, + "y": -0.0012320821406319737, + "z": 9.582709026290104e-06 + }, + "angular": { + "x": 0.00240899296477437, + "y": -0.0008162388112396002, + "z": -0.002154385903850198 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0014401900116354227, + "y": 0.0002597111160866916, + "z": 9.584119652572554e-06 + }, + "angular": { + "x": -0.0010410434333607554, + "y": 0.00232071615755558, + "z": -0.002154385903850198 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.005514512769877911, + "y": 0.0008513019420206547, + "z": -0.0026912905741482973 + }, + "angular": { + "x": -0.050422780215740204, + "y": 0.01784709095954895, + "z": 0.00641250517219305 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.00312848761677742, + "y": -0.004620298743247986, + "z": -0.0026912882458418608 + }, + "angular": { + "x": 0.021231483668088913, + "y": -0.049093786627054214, + "z": 0.00641250517219305 + } + }, + "gripper_open_percent": 96.4688491821289, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0006249356083571911, + "y": 0.00040508952224627137, + "z": -6.483352990471758e-06 + }, + "angular": { + "x": -0.0023317039012908936, + "y": -0.003260255791246891, + "z": -0.0028386192861944437 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0007220919942483306, + "y": 0.00018230880959890783, + "z": -6.468469564424595e-06 + }, + "angular": { + "x": 0.00397570850327611, + "y": 0.0005097439279779792, + "z": -0.0028386192861944437 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0006022657034918666, + "y": 0.002768540056422353, + "z": -0.002900382038205862 + }, + "angular": { + "x": -0.0026558050885796547, + "y": -0.0059717921540141106, + "z": 0.0015959467273205519 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0016189892776310444, + "y": -0.00232517858967185, + "z": -0.0029003676027059555 + }, + "angular": { + "x": 0.006183256395161152, + "y": 0.002117288764566183, + "z": 0.0015959467273205519 + } + }, + "gripper_open_percent": 1.0675549507141113, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0005715411389246583, + "y": -0.0003199128550477326, + "z": -7.23680841474561e-06 + }, + "angular": { + "x": -6.85802660882473e-06, + "y": -0.002602809574455023, + "z": 0.0013826705981045961 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0006233312888070941, + "y": -0.00020114483777433634, + "z": -7.244266271300148e-06 + }, + "angular": { + "x": 0.0019120078068226576, + "y": 0.001766039291396737, + "z": 0.0013826705981045961 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0016593028558418155, + "y": 0.001565505051985383, + "z": -0.005460038315504789 + }, + "angular": { + "x": -0.019902121275663376, + "y": -0.027951914817094803, + "z": 0.0010127808200195432 + } + }, + "arm_state_vision": { + "linear": { + "x": -1.814388087950647e-05, + "y": -0.0022811780218034983, + "z": -0.005460045300424099 + }, + "angular": { + "x": 0.03402544558048248, + "y": 0.004435411654412746, + "z": 0.001012780936434865 + } + }, + "gripper_open_percent": 1.0347306728363037, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0009672922315075994, + "y": -0.0002598768915049732, + "z": -0.00014461200044024736 + }, + "angular": { + "x": -0.0003426939947530627, + "y": -0.0018832539208233356, + "z": 0.0007941712974570692 + } + }, + "body_state_vision": { + "linear": { + "x": 0.0008486195001751184, + "y": -0.0005320083582773805, + "z": -0.00014463250408880413 + }, + "angular": { + "x": 0.001613236148841679, + "y": 0.0010303232120350003, + "z": 0.0007941712974570692 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0036022192798554897, + "y": -0.00015290104784071445, + "z": 0.0077107991091907024 + }, + "angular": { + "x": -0.0021148137748241425, + "y": -0.024595674127340317, + "z": 0.0014731344999745488 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.002563146408647299, + "y": -0.002535676583647728, + "z": 0.007710780017077923 + }, + "angular": { + "x": 0.01946275122463703, + "y": 0.015186230652034283, + "z": 0.001473134383559227 + } + }, + "gripper_open_percent": 0.5813062191009521, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.02991463616490364, + "y": 0.035833410918712616, + "z": -0.0819830447435379 + }, + "angular": { + "x": -0.04758603125810623, + "y": 0.04302965849637985, + "z": -0.010561290197074413 + } + }, + "body_state_vision": { + "linear": { + "x": -0.04661395773291588, + "y": -0.0024611600674688816, + "z": -0.08198323100805283 + }, + "angular": { + "x": 0.0008474234491586685, + "y": -0.06415026634931564, + "z": -0.010561289265751839 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.3364516496658325, + "y": -0.40240055322647095, + "z": -0.008286502212285995 + }, + "angular": { + "x": -0.7145630717277527, + "y": 0.4477505087852478, + "z": -0.11116410046815872 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.5238155126571655, + "y": 0.027258720248937607, + "z": -0.00828658789396286 + }, + "angular": { + "x": 0.15810775756835938, + "y": -0.8283007740974426, + "z": -0.11116408556699753 + } + }, + "gripper_open_percent": 0.5492508411407471, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 0.0026407414115965366, + "y": 0.0018394817598164082, + "z": 0.00013346273044589907 + }, + "angular": { + "x": -0.04250316694378853, + "y": 0.01251387968659401, + "z": 0.004874246194958687 + } + }, + "body_state_vision": { + "linear": { + "x": -0.003144844900816679, + "y": 0.0006834767991676927, + "z": 0.00013342265447136015 + }, + "angular": { + "x": 0.019750814884901047, + "y": -0.039661332964897156, + "z": 0.004874246194958687 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.006903793662786484, + "y": 0.008390475995838642, + "z": -0.6272764205932617 + }, + "angular": { + "x": -0.24804995954036713, + "y": 0.26492372155189514, + "z": 0.03396229073405266 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.0014508925378322601, + "y": -0.010768342763185501, + "z": -0.6272764205932617 + }, + "angular": { + "x": -0.025352314114570618, + "y": -0.36203667521476746, + "z": 0.03396228700876236 + } + }, + "gripper_open_percent": 0.5026936531066895, + "stow_state": 2 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": 8.42669396661222e-05, + "y": 0.0007354793488048017, + "z": -0.0009178121690638363 + }, + "angular": { + "x": 0.0022760224528610706, + "y": 0.0062278034165501595, + "z": 0.005229065660387278 + } + }, + "body_state_vision": { + "linear": { + "x": -0.0005962989525869489, + "y": -0.0004386992077343166, + "z": -0.0009178196196444333 + }, + "angular": { + "x": -0.006112441886216402, + "y": -0.002569795586168766, + "z": 0.005229065660387278 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.0021656081080436707, + "y": 0.0029021285008639097, + "z": -0.005291077308356762 + }, + "angular": { + "x": -0.003906608559191227, + "y": 9.168637916445732e-06, + "z": 0.013941949233412743 + } + }, + "arm_state_vision": { + "linear": { + "x": -0.000653108349069953, + "y": -0.003561690915375948, + "z": -0.005291084758937359 + }, + "angular": { + "x": 0.002651507966220379, + "y": -0.00286900345236063, + "z": 0.013941949233412743 + } + }, + "gripper_open_percent": 0.5362749099731445, + "stow_state": 1 + }, + { + "images": {}, + "body_state_odom": { + "linear": { + "x": -0.0719292014837265, + "y": -0.09986796975135803, + "z": -0.18480078876018524 + }, + "angular": { + "x": -0.029106052592396736, + "y": 0.014754699543118477, + "z": -0.006725664716213942 + } + }, + "body_state_vision": { + "linear": { + "x": 0.12212716042995453, + "y": 0.015244766138494015, + "z": -0.18480078876018524 + }, + "angular": { + "x": 0.008992777206003666, + "y": -0.03136870637536049, + "z": -0.006725664716213942 + } + }, + "arm_state_odom": { + "linear": { + "x": -0.06499375402927399, + "y": -0.09450909495353699, + "z": -0.204449862241745 + }, + "angular": { + "x": -0.005394976586103439, + "y": 0.05171635001897812, + "z": -0.0063977292738854885 + } + }, + "arm_state_vision": { + "linear": { + "x": 0.1134810522198677, + "y": 0.016680672764778137, + "z": -0.20444989204406738 + }, + "angular": { + "x": -0.03422681242227554, + "y": -0.039143532514572144, + "z": -0.006397728808224201 + } + }, + "gripper_open_percent": 0.5477249622344971, + "stow_state": 1 + } +] \ No newline at end of file diff --git a/spot/spot_env.txt b/spot/spot_env.txt new file mode 100644 index 00000000..a5c27315 --- /dev/null +++ b/spot/spot_env.txt @@ -0,0 +1,275 @@ +# This file may be used to create an environment using: +# $ conda create --name --file +# platform: linux-64 +_libgcc_mutex=0.1=main +_openmp_mutex=5.1=1_gnu +absl-py=1.4.0=pypi_0 +addict=2.4.0=pypi_0 +aiofiles=22.1.0=pypi_0 +aiosqlite=0.18.0=pypi_0 +anyio=3.6.2=pypi_0 +appdirs=1.4.4=pyhd3eb1b0_0 +argon2-cffi=21.3.0=pypi_0 +argon2-cffi-bindings=21.2.0=pypi_0 +arrow=1.2.3=pypi_0 +asttokens=2.2.1=pypi_0 +astunparse=1.6.3=pypi_0 +attrs=22.2.0=pypi_0 +babel=2.12.1=pypi_0 +backcall=0.2.0=pyhd3eb1b0_0 +beautifulsoup4=4.11.2=pypi_0 +blas=1.0=mkl +bleach=6.0.0=pypi_0 +blis=0.4.1=pypi_0 +bosdyn-api=3.2.3=pypi_0 +bosdyn-choreography-client=3.2.3=pypi_0 +bosdyn-choreography-protos=3.2.3=pypi_0 +bosdyn-client=3.2.3=pypi_0 +bosdyn-core=3.2.3=pypi_0 +bosdyn-mission=3.2.3=pypi_0 +brotli=1.0.9=h5eee18b_7 +brotli-bin=1.0.9=h5eee18b_7 +brotlipy=0.7.0=py38h27cfd23_1003 +ca-certificates=2023.01.10=h06a4308_0 +cachetools=5.3.0=pypi_0 +catalogue=1.0.2=pypi_0 +certifi=2022.12.7=py38h06a4308_0 +cffi=1.15.1=py38h74dc2b5_0 +charset-normalizer=3.0.1=pypi_0 +click=8.1.3=pypi_0 +clip=1.0=pypi_0 +comm=0.1.2=py38h06a4308_0 +confection=0.0.4=pypi_0 +configargparse=1.5.3=pypi_0 +contourpy=1.0.7=pypi_0 +cryptography=39.0.1=py38h9ce1e76_0 +cycler=0.11.0=pyhd3eb1b0_0 +cymem=2.0.7=pypi_0 +dash=2.8.1=pypi_0 +dash-core-components=2.0.0=pypi_0 +dash-html-components=2.0.0=pypi_0 +dash-table=5.0.0=pypi_0 +debugpy=1.6.6=pypi_0 +decorator=5.1.1=pyhd3eb1b0_0 +defusedxml=0.7.1=pypi_0 +deprecated=1.2.13=pypi_0 +distlib=0.3.4=pypi_0 +easydict=1.10=pypi_0 +en-core-web-sm=2.2.0=pypi_0 +entrypoints=0.4=py38h06a4308_0 +executing=1.2.0=pypi_0 +fancycompleter=0.9.1=pypi_0 +fastjsonschema=2.16.2=pypi_0 +filelock=3.6.0=pypi_0 +flask=2.2.3=pypi_0 +flatbuffers=23.1.21=pypi_0 +fonttools=4.38.0=pypi_0 +fqdn=1.5.1=pypi_0 +freetype=2.12.1=h4a9f257_0 +fsspec=2023.4.0=pypi_0 +ftfy=6.1.1=pypi_0 +gast=0.4.0=pypi_0 +giflib=5.2.1=h5eee18b_3 +google-auth=2.16.0=pypi_0 +google-auth-oauthlib=0.4.6=pypi_0 +google-pasta=0.2.0=pypi_0 +grpcio=1.51.1=pypi_0 +h5py=3.8.0=pypi_0 +huggingface-hub=0.14.1=pypi_0 +idna=3.4=py38h06a4308_0 +imageio=2.28.1=pypi_0 +importlib-metadata=6.0.0=pypi_0 +importlib-resources=5.12.0=pypi_0 +intel-openmp=2021.4.0=h06a4308_3561 +ipykernel=6.21.2=pypi_0 +ipython=8.10.0=pypi_0 +ipython-genutils=0.2.0=pypi_0 +ipywidgets=8.0.4=pypi_0 +isoduration=20.11.0=pypi_0 +itsdangerous=2.1.2=pypi_0 +jedi=0.18.2=pypi_0 +jinja2=3.1.2=pypi_0 +joblib=1.2.0=pypi_0 +jpeg=9e=h5eee18b_1 +json5=0.9.11=pypi_0 +jsonpointer=2.3=pypi_0 +jsonschema=4.17.3=pypi_0 +jupyter-client=8.0.3=pypi_0 +jupyter-core=5.2.0=pypi_0 +jupyter-events=0.6.3=pypi_0 +jupyter-server=2.4.0=pypi_0 +jupyter-server-fileid=0.8.0=pypi_0 +jupyter-server-terminals=0.4.4=pypi_0 +jupyter-server-ydoc=0.6.1=pypi_0 +jupyter-ydoc=0.2.3=pypi_0 +jupyterlab=3.6.1=pypi_0 +jupyterlab-pygments=0.2.2=pypi_0 +jupyterlab-server=2.20.0=pypi_0 +jupyterlab-widgets=3.0.5=pypi_0 +keras=2.11.0=pypi_0 +kiwisolver=1.4.4=py38h6a678d5_0 +langcodes=3.3.0=pypi_0 +lazy-loader=0.2=pypi_0 +lcms2=2.12=h3be6417_0 +ld_impl_linux-64=2.38=h1181459_1 +lerc=3.0=h295c915_0 +libbrotlicommon=1.0.9=h5eee18b_7 +libbrotlidec=1.0.9=h5eee18b_7 +libbrotlienc=1.0.9=h5eee18b_7 +libclang=15.0.6.1=pypi_0 +libdeflate=1.17=h5eee18b_0 +libffi=3.3=he6710b0_2 +libgcc-ng=11.2.0=h1234567_1 +libgfortran-ng=11.2.0=h00389a5_1 +libgfortran5=11.2.0=h1234567_1 +libgomp=11.2.0=h1234567_1 +libpng=1.6.39=h5eee18b_0 +libsodium=1.0.18=h7b6447c_0 +libstdcxx-ng=11.2.0=h1234567_1 +libtiff=4.5.0=h6a678d5_2 +libwebp=1.2.4=h11a3e52_1 +libwebp-base=1.2.4=h5eee18b_1 +lz4-c=1.9.4=h6a678d5_0 +markdown=3.4.1=pypi_0 +markupsafe=2.1.2=pypi_0 +matplotlib=3.6.3=pypi_0 +matplotlib-inline=0.1.6=py38h06a4308_0 +mistune=2.0.5=pypi_0 +mkl=2021.4.0=h06a4308_640 +mkl-service=2.4.0=py38h7f8727e_0 +mkl_fft=1.3.1=py38hd3c417c_0 +mkl_random=1.2.2=py38h51133e4_0 +munkres=1.1.4=py_0 +murmurhash=1.0.9=pypi_0 +nbclassic=0.5.3=pypi_0 +nbclient=0.7.2=pypi_0 +nbconvert=7.2.10=pypi_0 +nbformat=5.5.0=pypi_0 +ncurses=6.4=h6a678d5_0 +nest-asyncio=1.5.6=py38h06a4308_0 +networkx=3.1=pypi_0 +nltk=3.8.1=pypi_0 +notebook=6.5.3=pypi_0 +notebook-shim=0.2.2=pypi_0 +numpy=1.23.1=pypi_0 +numpy-base=1.23.5=py38h31eccc5_0 +nvidia-cublas-cu11=11.10.3.66=pypi_0 +nvidia-cuda-nvrtc-cu11=11.7.99=pypi_0 +nvidia-cuda-runtime-cu11=11.7.99=pypi_0 +nvidia-cudnn-cu11=8.5.0.96=pypi_0 +oauthlib=3.2.2=pypi_0 +open3d=0.16.0=pypi_0 +opencv-python-headless=4.7.0.72=pypi_0 +openssl=1.1.1s=h7f8727e_0 +opt-einsum=3.3.0=pypi_0 +packaging=23.0=pypi_0 +pandas=1.5.3=pypi_0 +pandocfilters=1.5.0=pypi_0 +parso=0.8.3=pyhd3eb1b0_0 +pathy=0.10.1=pypi_0 +pexpect=4.8.0=pyhd3eb1b0_3 +pickleshare=0.7.5=pyhd3eb1b0_1003 +pillow=9.3.0=pypi_0 +pip=23.0.1=py38h06a4308_0 +pkgutil-resolve-name=1.3.10=pypi_0 +plac=1.1.3=pypi_0 +platformdirs=2.5.1=pypi_0 +plotly=5.13.0=pypi_0 +pooch=1.4.0=pyhd3eb1b0_0 +preshed=3.0.8=pypi_0 +prometheus-client=0.16.0=pypi_0 +prompt-toolkit=3.0.36=py38h06a4308_0 +protobuf=3.19.6=pypi_0 +psutil=5.9.4=pypi_0 +ptyprocess=0.7.0=pyhd3eb1b0_2 +pure_eval=0.2.2=pyhd3eb1b0_0 +pyasn1=0.4.8=pypi_0 +pyasn1-modules=0.2.8=pypi_0 +pycparser=2.21=pyhd3eb1b0_0 +pydantic=1.10.7=pypi_0 +pygments=2.14.0=pypi_0 +pyjwt=2.6.0=pypi_0 +pyopenssl=23.0.0=py38h06a4308_0 +pyparsing=3.0.9=py38h06a4308_0 +pyqt5=5.15.9=pypi_0 +pyqt5-qt5=5.15.2=pypi_0 +pyqt5-sip=12.11.1=pypi_0 +pyquaternion=0.9.9=pypi_0 +pyrepl=0.9.0=pypi_0 +pyrsistent=0.19.3=pypi_0 +pysocks=1.7.1=py38h06a4308_0 +python=3.8.10=h12debd9_8 +python-dateutil=2.8.2=pyhd3eb1b0_0 +python-json-logger=2.0.7=pypi_0 +pytz=2022.7.1=pypi_0 +pywavelets=1.4.1=pypi_0 +pyyaml=6.0=pypi_0 +pyzmq=25.0.0=pypi_0 +readline=8.2=h5eee18b_0 +regex=2022.10.31=pypi_0 +requests=2.29.0=pypi_0 +requests-oauthlib=1.3.1=pypi_0 +rfc3339-validator=0.1.4=pypi_0 +rfc3986-validator=0.1.1=pypi_0 +rsa=4.9=pypi_0 +scikit-image=0.20.0=pypi_0 +scikit-learn=1.2.1=pypi_0 +scipy=1.9.1=pypi_0 +send2trash=1.8.0=pypi_0 +sentence-transformers=2.2.2=pypi_0 +sentencepiece=0.1.98=pypi_0 +setuptools=65.6.3=py38h06a4308_0 +six=1.16.0=pyhd3eb1b0_1 +smart-open=6.3.0=pypi_0 +sniffio=1.3.0=pypi_0 +soupsieve=2.4=pypi_0 +spacy=2.2.4=pypi_0 +spacy-legacy=3.0.12=pypi_0 +spacy-loggers=1.0.4=pypi_0 +sqlite=3.40.1=h5082296_0 +srsly=1.0.6=pypi_0 +stack-data=0.6.2=pypi_0 +tenacity=8.2.1=pypi_0 +tensorboard=2.11.2=pypi_0 +tensorboard-data-server=0.6.1=pypi_0 +tensorboard-plugin-wit=1.8.1=pypi_0 +tensorflow=2.11.0=pypi_0 +tensorflow-estimator=2.11.0=pypi_0 +tensorflow-io-gcs-filesystem=0.30.0=pypi_0 +termcolor=2.2.0=pypi_0 +terminado=0.17.1=pypi_0 +thinc=7.4.0=pypi_0 +threadpoolctl=3.1.0=pypi_0 +tifffile=2023.4.12=pypi_0 +tinycss2=1.2.1=pypi_0 +tk=8.6.12=h1ccaba5_0 +tokenizers=0.13.3=pypi_0 +tomli=2.0.1=pypi_0 +torch=1.7.1=pypi_0 +torchvision=0.8.2=pypi_0 +tornado=6.2=py38h5eee18b_0 +tqdm=4.64.1=pypi_0 +traitlets=5.9.0=pypi_0 +transformers=4.28.1=pypi_0 +typer=0.7.0=pypi_0 +typing-extensions=4.5.0=pypi_0 +uri-template=1.2.0=pypi_0 +urllib3=1.26.14=py38h06a4308_0 +virtualenv=20.13.2=pypi_0 +wasabi=0.10.1=pypi_0 +wcwidth=0.2.6=pypi_0 +webcolors=1.12=pypi_0 +webencodings=0.5.1=pypi_0 +websocket-client=1.5.1=pypi_0 +werkzeug=2.2.2=pypi_0 +wheel=0.38.4=py38h06a4308_0 +widgetsnbextension=4.0.5=pypi_0 +wmctrl=0.4=pypi_0 +wrapt=1.14.1=pypi_0 +xz=5.2.10=h5eee18b_1 +y-py=0.5.9=pypi_0 +ypy-websocket=0.8.2=pypi_0 +zeromq=4.3.4=h2531618_0 +zipp=3.12.1=pypi_0 +zlib=1.2.13=h5eee18b_0 +zstd=1.5.2=ha4553b6_0 diff --git a/spot/spot_utils/README.md b/spot/spot_utils/README.md new file mode 100644 index 00000000..050ba953 --- /dev/null +++ b/spot/spot_utils/README.md @@ -0,0 +1,53 @@ +This is a set of utility functions that are useful for interfacing with the spot. + +## Camera Intrinsics +Getting the camera intrinsics is important for relating image coordinates to world coordinates (within the camera frame). You can use + +`python3 get_intrinsics.py ROBOT_IP --image-sources IMAGE_SOURCE` + +And the focal length, principal points, and skew should print out. For example, you can run + +`python3 get_intrinsics.py 138.16.161.12 --image-sources hand_color_image` + +for the spot rgb hand camera, the focal length for x and y may be 552.0291012161067, and principal x,y may be (320, 240), 0 skew. The relevant intrinsics matrix would be: + +`intrinsics_matrix = np.array([[552.0291012161067, 0 ,320.0],[0,552.0291012161067,240.0],[0,0,1]])` + +## Collect depth + color images and robot poses +You can teleoperate the spot using the controller, and then run the following program: + +`python get_depth_color_pose.py --manual_images [True/False] --path_dir [DIR_PATH] --dir_name [DIR_NAME] [ROBOT_IP]` + +For example: + +`python get_depth_color_pose.py --manual_images True --path_dir ../data --dir_name spot_data 138.16.161.12` + +I suggest making sure the robot hand is open so that the depth camera is blocked less. You will be prompted to take a photo all the time. Move the robot where you want and then hit enter. The program will capture the depth and color image, convert the depth image to be in color image space, and then get the robot pose in the vision frame. All of this is then saved to a folder. If you'd prefer the robot to continuously take images at a fixed frequency, pass False to the manual_images flag. path_dir is the path to where the new data directory will be located, and dir_name is the name of the new directory for the data. + +When manual_images is False, the robot will only take photos after it has moved and is currently still (to reduce the number of blurry and unneeded photos). If high precision is required, use manual mode and take images when the robot is still. + +## Generate pointcloud from data +After collecting depth, color and pose data, we can generate a point cloud of the scene, along with axes representing the pose of the robot's hand in the vision frame. In addition, we can save the pointcloud file in the same location we have the sensor data + +`python generate_pointcloud.py` + +## Move-to Location +If you want to stand the robot up and move 2 meters within a given a location in the vision frame, you can check out + +`python move_spot_to.py [ROBOT_IP]` + +## E-stop +You'll need to run an e-stop if you want the robot to operate autonomously, I suggest looking at the example code from the Spot SDK: + +https://github.com/boston-dynamics/spot-sdk/tree/master/python/examples/estop + +## TODO +- make generate point cloud way more effecient by using transform with position and rotation data on pcd rather than add apply to each point individually +- get_depth_color_pose.py should have options for visual vs. map frame +- arguments should be passed in at command line instead of hard coded +- move_spot_to has temporary "stupid" class, and should have adjustable approach range, and also handle obstacles, and also connect to seed frame +- timeout for move_spot_to should be considered more, also make navigation more advance (todo above) + +## BUGS +- (?) I am using the hand frame for get_depth_color_pose.py but I should double check that this is aligned with hand color frame + - Seems like it's not the same exact frame, but there is an off-set that is calculated in visualize_data.py currently, move it somewhere else \ No newline at end of file diff --git a/spot/spot_utils/generate_pointcloud.py b/spot/spot_utils/generate_pointcloud.py new file mode 100644 index 00000000..21873bd9 --- /dev/null +++ b/spot/spot_utils/generate_pointcloud.py @@ -0,0 +1,67 @@ +import pickle +import matplotlib.pyplot as plt +import math +import numpy as np +import os +import cv2 +import open3d as o3d +from tqdm import tqdm +from spot_utils.utils import pixel_to_vision_frame + +def make_pointcloud(data_path="../data/spot-depth-color-pose-data3/", pose_data_fname="pose_data.pkl", pointcloud_fname="pointcloud.pcd"): + fill_in = False #if fill_in is True, then gaps in depth image are filled in with black (at maximal distance..?) + save_pc = True #if true, save point cloud to same location as dir_path+dir_name + + pose_dir = pickle.load(open(f"{data_path}{pose_data_fname}","rb")) + + ####################################### + # Visualize point cloud + file_names = os.listdir(data_path) + num_files = int((len(file_names)-1)/ 3.0) + total_pcds = [] + total_colors = [] + total_axes = [] + for file_num in tqdm(range(num_files)): + if file_num == 6: + continue + rotation_matrix = pose_dir[file_num]['rotation_matrix'] + position = pose_dir[file_num]['position'] + + color_img = cv2.imread(f"{data_path}color_{str(file_num)}.jpg") + color_img = color_img[:,:,::-1] # RGB-> BGR + depth_img = pickle.load(open(f"{data_path}depth_{str(file_num)}","rb"))#cv2.imread(dir_path+dir_name+"depth_"+str(file_num)+".jpg") + + H,W = depth_img.shape + for i in range(H): + for j in range(W): + #first apply rot2 to move camera into hand frame, then apply rotation + transform of hand frame in vision frame + transformed_xyz,_ = pixel_to_vision_frame(i,j,depth_img,rotation_matrix,position) + + total_pcds.append(transformed_xyz) + + # Add the color of the pixel if it exists: + if 0 <= j < W and 0 <= i < H: + total_colors.append(color_img[i,j] / 255) + elif fill_in: + total_colors.append([0., 0., 0.]) + + mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6,origin=[0,0,0]) + mesh_frame = mesh_frame.rotate(rotation_matrix, center=(0, 0, 0)).translate(position) + #mesh_frame.paint_uniform_color([float(file_num)/num_files, 0.1, 1-(float(file_num)/num_files)]) + + total_axes.append(mesh_frame) + + pcd_o3d = o3d.geometry.PointCloud() # create a point cloud object + pcd_o3d.points = o3d.utility.Vector3dVector(total_pcds) + pcd_o3d.colors = o3d.utility.Vector3dVector(total_colors) + + #bb = o3d.geometry.OrientedBoundingBox(center=np.array([0,0,0]),R=rot2_mat,extent=np.array([1,1,1])) + + # Visualize: + origin_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1,origin=[0,0,0]) + o3d.visualization.draw_geometries([pcd_o3d]+total_axes+[origin_frame]) + + if save_pc: + o3d.io.write_point_cloud(f"{data_path}{pointcloud_fname}", pcd_o3d) + + return(pcd_o3d) diff --git a/spot/spot_utils/get_depth_color_pose.py b/spot/spot_utils/get_depth_color_pose.py new file mode 100644 index 00000000..890a5512 --- /dev/null +++ b/spot/spot_utils/get_depth_color_pose.py @@ -0,0 +1,167 @@ +# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +"""Example demonstrating capture of both visual and depth images and then overlaying them.""" + +import argparse + +import sys +import os +import time + +import bosdyn.client +import bosdyn.client.util +from bosdyn.client.image import ImageClient + +import cv2 +import numpy as np + + +def main(argv): + # Parse args + + parser = argparse.ArgumentParser() + bosdyn.client.util.add_base_arguments(parser) + parser.add_argument("--manual_images", help="Whether images are taken manually or continuously",default="True") + parser.add_argument("--path_dir", help="path to directory where data is saved",default="../data") + parser.add_argument("--dir_name", help="name of directory for where data is stored",default="default_data") + options = parser.parse_args(argv) + + if options.manual_images == "True": + manual_images = True + else: + already_took_photo = False + manual_images = False + robot_is_moving = False + robot_take_pic = True + last_robot_position = [0,0,0] + + + visualize = False #everytime we capture image, vis the results + robot_pose = True #get camera frame in vision frame when picture taken + img_dir = options.path_dir + "/" + options.dir_name + "/" + pic_hz = 2 #if manual_images is false, the hz at which images are automatically taken + + if visualize: + from matplotlib import pyplot as plt + if robot_pose: + from bosdyn.client.frame_helpers import get_a_tform_b, get_frame_names + import pickle + + if not os.path.isdir(img_dir): + os.mkdir(img_dir) + + + #We only use the hand color + sources = ['hand_depth_in_hand_color_frame', 'hand_color_image'] + + # Create robot object with an image client. + sdk = bosdyn.client.create_standard_sdk('image_depth_plus_visual') + robot = sdk.create_robot(options.hostname) + bosdyn.client.util.authenticate(robot) + image_client = robot.ensure_client(ImageClient.default_service_name) + + counter = 0 + img_to_pose_dir = {} #takes in counter as key, and returns robot pose. saved in img_dir + while True: + if manual_images: + response = input("Take image [y/n]") + if response == "n": + break + else: + time.sleep(1/float(pic_hz)) + + # Capture and save images to disk + image_responses = image_client.get_image_from_sources(sources) + + if robot_pose: + frame_tree_snapshot = robot.get_frame_tree_snapshot() + vision_tform_hand = get_a_tform_b(frame_tree_snapshot,"vision","hand") + + img_to_pose_dir[counter] = {"position": [vision_tform_hand.position.x,vision_tform_hand.position.y,vision_tform_hand.position.z], + "quaternion(wxyz)": [vision_tform_hand.rotation.w,vision_tform_hand.rotation.x,vision_tform_hand.rotation.y,vision_tform_hand.rotation.z], + "rotation_matrix": vision_tform_hand.rotation.to_matrix(), + "rpy": [vision_tform_hand.rotation.to_roll(),vision_tform_hand.rotation.to_pitch(),vision_tform_hand.rotation.to_yaw()]} + + pickle.dump(img_to_pose_dir,open(img_dir+"pose_data.pkl","wb")) + + robot_position = [vision_tform_hand.position.x,vision_tform_hand.position.y,vision_tform_hand.position.z] + + if not manual_images: + if np.linalg.norm(np.array(robot_position) - np.array(last_robot_position)) > 0.2: #robot is moving around + print("Robot is moving, no photos!") + last_robot_position = robot_position + already_took_photo = False + continue + else: + last_robot_position = robot_position + if already_took_photo: #don't take a phot if you already have + print("Robot still but already took photo") + continue + else: + print("Robot still: NEW PHOTO!") + already_took_photo = True #going to take photo, don't next time + + + + # Image responses are in the same order as the requests. + # Convert to opencv images. + + if len(image_responses) < 2: + print('Error: failed to get images.') + return False + + # Depth is a raw bytestream + cv_depth = np.frombuffer(image_responses[0].shot.image.data, dtype=np.uint16) + cv_depth = cv_depth.reshape(image_responses[0].shot.image.rows, + image_responses[0].shot.image.cols) + + #cv_depth is in millimeters, divide by 1000 to get it into meters + cv_depth_meters = cv_depth / 1000.0 + + # Visual is a JPEG + cv_visual = cv2.imdecode(np.frombuffer(image_responses[1].shot.image.data, dtype=np.uint8), -1) + + #cv2.imwrite("color.jpg", cv_visual) + + # Convert the visual image from a single channel to RGB so we can add color + visual_rgb = cv_visual if len(cv_visual.shape) == 3 else cv2.cvtColor(cv_visual, cv2.COLOR_GRAY2RGB) + + + + # Map depth ranges to color + + # cv2.applyColorMap() only supports 8-bit; convert from 16-bit to 8-bit and do scaling + min_val = np.min(cv_depth) + max_val = np.max(cv_depth) + depth_range = max_val - min_val + depth8 = (255.0 / depth_range * (cv_depth - min_val)).astype('uint8') + depth8_rgb = cv2.cvtColor(depth8, cv2.COLOR_GRAY2RGB) + depth_color = cv2.applyColorMap(depth8_rgb, cv2.COLORMAP_JET) + + # Add the two images together. + out = cv2.addWeighted(visual_rgb, 0.5, depth_color, 0.5, 0) + + cv2.imwrite(img_dir+"color_"+str(counter)+".jpg", cv_visual) + pickle.dump(cv_depth_meters, open(img_dir+"depth_"+str(counter),"wb")) + cv2.imwrite(img_dir+"combined_"+str(counter)+".jpg", out) + counter += 1 + + if visualize: + fig, axs = plt.subplots(1, 3) + axs[0].set_title("Depth (in meters)") + axs[0].imshow(cv_depth_meters) + axs[1].set_title("Color") + axs[1].imshow(cv_visual) + axs[2].set_title("Depth + Color") + axs[2].imshow(out) + plt.show() + + + +if __name__ == "__main__": + if not main(sys.argv[1:]): + sys.exit(1) diff --git a/spot/spot_utils/get_intrinsics.py b/spot/spot_utils/get_intrinsics.py new file mode 100644 index 00000000..b7f8dbe2 --- /dev/null +++ b/spot/spot_utils/get_intrinsics.py @@ -0,0 +1,133 @@ +# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +"""Simple image capture tutorial.""" + +import argparse +import sys + +from bosdyn.api import image_pb2 +import bosdyn.client +import bosdyn.client.util +from bosdyn.client.image import ImageClient, build_image_request +from bosdyn.api import image_pb2 +import cv2 +import numpy as np + +from scipy import ndimage + +ROTATION_ANGLE = { + 'back_fisheye_image': 0, + 'frontleft_fisheye_image': -78, + 'frontright_fisheye_image': -102, + 'left_fisheye_image': 0, + 'right_fisheye_image': 180 +} + + +def pixel_format_type_strings(): + names = image_pb2.Image.PixelFormat.keys() + return names[1:] + + +def pixel_format_string_to_enum(enum_string): + return dict(image_pb2.Image.PixelFormat.items()).get(enum_string) + + +def main(argv): + # Parse args + parser = argparse.ArgumentParser() + bosdyn.client.util.add_base_arguments(parser) + parser.add_argument('--list', help='list image sources', action='store_true') + parser.add_argument('--auto-rotate', help='rotate right and front images to be upright', + action='store_true') + parser.add_argument('--image-sources', help='Get image from source(s)', action='append') + parser.add_argument('--image-service', help='Name of the image service to query.', + default=ImageClient.default_service_name) + parser.add_argument( + '--pixel-format', choices=pixel_format_type_strings(), + help='Requested pixel format of image. If supplied, will be used for all sources.') + + options = parser.parse_args(argv) + + # Create robot object with an image client. + sdk = bosdyn.client.create_standard_sdk('image_capture') + robot = sdk.create_robot(options.hostname) + bosdyn.client.util.authenticate(robot) + robot.sync_with_directory() + robot.time_sync.wait_for_sync() + + image_client = robot.ensure_client(options.image_service) + + # Raise exception if no actionable argument provided + if not options.list and not options.image_sources: + parser.error('Must provide actionable argument (list or image-sources).') + + image_sources = image_client.list_image_sources() + print("Image sources:") + for source in image_sources: + if source.name in options.image_sources: + print("\t" + source.name) + intrinsics = source.pinhole.intrinsics + print(f"focal length: {intrinsics.focal_length}") + print(f"principal_point: {intrinsics.principal_point}") + print(f"skew: {intrinsics.skew}") + + ''' + # Optionally capture one or more images. + if options.image_sources: + # Capture and save images to disk + pixel_format = pixel_format_string_to_enum(options.pixel_format) + image_request = [ + build_image_request(source, pixel_format=pixel_format) + for source in options.image_sources + ] + image_responses = image_client.get_image(image_request) + + for image in image_responses: + num_bytes = 1 # Assume a default of 1 byte encodings. + if image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: + dtype = np.uint16 + extension = ".png" + else: + if image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_RGB_U8: + num_bytes = 3 + elif image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_RGBA_U8: + num_bytes = 4 + elif image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8: + num_bytes = 1 + elif image.shot.image.pixel_format == image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U16: + num_bytes = 2 + dtype = np.uint8 + extension = ".jpg" + + img = np.frombuffer(image.shot.image.data, dtype=dtype) + if image.shot.image.format == image_pb2.Image.FORMAT_RAW: + try: + # Attempt to reshape array into a RGB rows X cols shape. + img = img.reshape((image.shot.image.rows, image.shot.image.cols, num_bytes)) + except ValueError: + # Unable to reshape the image data, trying a regular decode. + img = cv2.imdecode(img, -1) + else: + img = cv2.imdecode(img, -1) + + if options.auto_rotate: + img = ndimage.rotate(img, ROTATION_ANGLE[image.source.name]) + + # Save the image from the GetImage request to the current directory with the filename + # matching that of the image source. + image_saved_path = image.source.name + image_saved_path = image_saved_path.replace( + "/", '') # Remove any slashes from the filename the image is saved at locally. + cv2.imwrite(image_saved_path + extension, img) + return True + ''' + + +if __name__ == "__main__": + if not main(sys.argv[1:]): + sys.exit(1) diff --git a/spot/spot_utils/move_spot_to.py b/spot/spot_utils/move_spot_to.py new file mode 100644 index 00000000..8c3c33d6 --- /dev/null +++ b/spot/spot_utils/move_spot_to.py @@ -0,0 +1,214 @@ +import sys +import argparse +import time +import numpy as np + +import bosdyn.api.gripper_command_pb2 +import bosdyn.client +import bosdyn.client.lease +import bosdyn.client.util + +from bosdyn.api import geometry_pb2 +from bosdyn.api import basic_command_pb2 + +from bosdyn.client.frame_helpers import VISION_FRAME_NAME, GRAV_ALIGNED_BODY_FRAME_NAME, get_a_tform_b +from bosdyn.client.robot_command import (RobotCommandBuilder, RobotCommandClient, + block_until_arm_arrives, blocking_stand) +from bosdyn.client.robot_state import RobotStateClient +from bosdyn.client import math_helpers + + +class stupid: + def __init__(self, x=3.80, y=2.743, z=0.564): + self.x = x + self.y = y + self.z = z + + +def simple_move_to(robot, position, rotation_matrix, end_time=30): + robot.logger.info("Powering on robot... This may take a several seconds.") + robot.power_on(timeout_sec=20) + + robot.logger.info("Commanding robot to stand...") + command_client = robot.ensure_client( + RobotCommandClient.default_service_name) + blocking_stand(command_client, timeout_sec=10) + robot.logger.info("Robot standing.") + + move_cmd = RobotCommandBuilder.trajectory_command( + goal_x=position[0], + goal_y=position[1], + goal_heading=math_helpers.Quat.from_matrix(rotation_matrix).to_yaw(), + frame_name=VISION_FRAME_NAME, + params=get_walking_params(0.5, 0.5)) + + print("Robot start moving!") + cmd_id = command_client.robot_command(command=move_cmd, + end_time_secs=time.time() + + end_time) + + # Wait until the robot reports that it is at the goal. + block_for_trajectory_cmd(command_client, cmd_id, + timeout_sec=10.0, verbose=True) + + +def move_to(robot, robot_state_client, pose=None, distance_margin=1.00, hostname="138.16.161.12", end_time=30): + robot.logger.info("Powering on robot... This may take a several seconds.") + robot.power_on(timeout_sec=20) + + robot.logger.info("Commanding robot to stand...") + command_client = robot.ensure_client( + RobotCommandClient.default_service_name) + blocking_stand(command_client, timeout_sec=10) + robot.logger.info("Robot standing.") + + #walk_rt_vision = [-0.6707368427993169, -0.6613703096820227, 0.2523603994192693] + # Walk to the object. + if type(pose) == type(None): + vision_tform_pose = stupid() + else: + vision_tform_pose = stupid(*pose) + + walk_rt_vision, heading_rt_vision = compute_stand_location_and_yaw( + vision_tform_pose, robot_state_client, distance_margin=distance_margin, robot=robot) + + move_cmd = RobotCommandBuilder.trajectory_command( + goal_x=walk_rt_vision[0], + goal_y=walk_rt_vision[1], + goal_heading=heading_rt_vision, + frame_name=VISION_FRAME_NAME, + params=get_walking_params(0.5, 0.5)) + + cmd_id = command_client.robot_command(command=move_cmd, + end_time_secs=time.time() + + end_time) + + # Wait until the robot reports that it is at the goal. + block_for_trajectory_cmd(command_client, cmd_id, + timeout_sec=10.0, verbose=True) + + +def compute_stand_location_and_yaw(vision_tform_target, robot_state_client, + distance_margin, robot): + # Compute drop-off location: + # Draw a line from Spot to the person + # Back up 2.0 meters on that line + ''' + vision_tform_robot = get_a_tform_b( + robot_state_client.get_robot_state( + ).kinematic_state.transforms_snapshot, VISION_FRAME_NAME, + GRAV_ALIGNED_BODY_FRAME_NAME) + ''' + + frame_tree_snapshot = robot.get_frame_tree_snapshot() + vision_tform_robot = get_a_tform_b(frame_tree_snapshot, "vision", "hand") + + # Compute vector between robot and person + robot_rt_person_ewrt_vision = [ + vision_tform_robot.x - vision_tform_target.x, + vision_tform_robot.y - vision_tform_target.y, + vision_tform_robot.z - vision_tform_target.z + ] + + # Compute the unit vector. + if np.linalg.norm(robot_rt_person_ewrt_vision) < 0.01: + robot_rt_person_ewrt_vision_hat = vision_tform_robot.transform_point( + 1, 0, 0) + else: + robot_rt_person_ewrt_vision_hat = robot_rt_person_ewrt_vision / np.linalg.norm( + robot_rt_person_ewrt_vision) + + # Starting at the person, back up meters along the unit vector. + drop_position_rt_vision = [ + vision_tform_target.x + + robot_rt_person_ewrt_vision_hat[0] * distance_margin, + vision_tform_target.y + + robot_rt_person_ewrt_vision_hat[1] * distance_margin, + vision_tform_target.z + + robot_rt_person_ewrt_vision_hat[2] * distance_margin + ] + + # We also want to compute a rotation (yaw) so that we will face the person when dropping. + # We'll do this by computing a rotation matrix with X along + # -robot_rt_person_ewrt_vision_hat (pointing from the robot to the person) and Z straight up: + xhat = -robot_rt_person_ewrt_vision_hat + zhat = [0.0, 0.0, 1.0] + yhat = np.cross(zhat, xhat) + mat = np.matrix([xhat, yhat, zhat]).transpose() + heading_rt_vision = math_helpers.Quat.from_matrix(mat).to_yaw() + + return drop_position_rt_vision, heading_rt_vision + + +def get_walking_params(max_linear_vel, max_rotation_vel): + max_vel_linear = geometry_pb2.Vec2(x=max_linear_vel, y=max_linear_vel) + max_vel_se2 = geometry_pb2.SE2Velocity(linear=max_vel_linear, + angular=max_rotation_vel) + vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2) + params = RobotCommandBuilder.mobility_params() + params.vel_limit.CopyFrom(vel_limit) + return params + + +def block_for_trajectory_cmd(command_client, cmd_id, timeout_sec=None, verbose=False): + """Helper that blocks until a trajectory command reaches STATUS_AT_GOAL or a timeout is + exceeded. + Args: + command_client: robot command client, used to request feedback + cmd_id: command ID returned by the robot when the trajectory command was sent + timeout_sec: optional number of seconds after which we'll return no matter what the + robot's state is. + verbose: if we should print state at 10 Hz. + Return values: + True if reaches STATUS_AT_GOAL, False otherwise. + """ + start_time = time.time() + + if timeout_sec is not None: + end_time = start_time + timeout_sec + now = time.time() + + while timeout_sec is None or now < end_time: + feedback_resp = command_client.robot_command_feedback(cmd_id) + + current_state = feedback_resp.feedback.mobility_feedback.se2_trajectory_feedback.status + + if verbose: + current_state_str = basic_command_pb2.SE2TrajectoryCommand.Feedback.Status.Name( + current_state) + + current_time = time.time() + print('Walking: ({time:.1f} sec): {state}'.format( + time=current_time - start_time, state=current_state_str), + end=' \r') + + if current_state == basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL: + return True + + time.sleep(0.1) + now = time.time() + + if verbose: + print('block_for_trajectory_cmd: timeout exceeded.') + + return False + + +if __name__ == '__main__': + sdk = bosdyn.client.create_standard_sdk('NLMapSpot') + robot = sdk.create_robot(hostname) + bosdyn.client.util.authenticate(robot) + + # Time sync is necessary so that time-based filter requests can be converted + robot.time_sync.wait_for_sync() + + assert not robot.is_estopped(), "Robot is estopped. Please use an external E-Stop client, " \ + "such as the estop SDK example, to configure E-Stop." + + robot_state_client = robot.ensure_client( + RobotStateClient.default_service_name) + + lease_client = robot.ensure_client( + bosdyn.client.lease.LeaseClient.default_service_name) + with bosdyn.client.lease.LeaseKeepAlive(lease_client, must_acquire=True, return_at_exit=True): + move_to(robot, robot_state_client) diff --git a/spot/spot_utils/utils.py b/spot/spot_utils/utils.py new file mode 100644 index 00000000..06ebf8a6 --- /dev/null +++ b/spot/spot_utils/utils.py @@ -0,0 +1,298 @@ +import numpy as np + +import bosdyn.client +import bosdyn.client.estop +import bosdyn.client.lease +import bosdyn.client.util +from bosdyn.api import estop_pb2, geometry_pb2, image_pb2, manipulation_api_pb2, robot_command_pb2 +from bosdyn.client.estop import EstopClient +from bosdyn.client.frame_helpers import VISION_FRAME_NAME, get_vision_tform_body, math_helpers +from bosdyn.client.image import ImageClient +from bosdyn.client.manipulation_api_client import ManipulationApiClient +from bosdyn.client.robot_command import RobotCommandClient, blocking_stand, RobotCommandBuilder +from bosdyn.client.robot_state import RobotStateClient +import time + + +def verify_estop(robot): + """Verify the robot is not estopped""" + + client = robot.ensure_client(EstopClient.default_service_name) + if client.get_status().stop_level != estop_pb2.ESTOP_LEVEL_NONE: + error_message = "Robot is estopped. Please use an external E-Stop client, such as the" \ + " estop SDK example, to configure E-Stop." + robot.logger.error(error_message) + raise Exception(error_message) + +def open_gripper(robot_command_client): + gripper_open = RobotCommandBuilder.claw_gripper_open_fraction_command(1.0) + cmd_id = robot_command_client.robot_command(gripper_open) + + + +def grasp_object(robot_state_client, manipulation_api_client, pick_pixels, image): + + # specify the pick location (coordinates) in the image + pick_vec = geometry_pb2.Vec2(x=pick_pixels[0], y=pick_pixels[1]) + + # Request Pick Up on that pixel. + grasp = manipulation_api_pb2.PickObjectInImage( + pixel_xy=pick_vec, + transforms_snapshot_for_camera=image.shot.transforms_snapshot, + frame_name_image_sensor=image.shot.frame_name_image_sensor, + camera_model=image.source.pinhole) + + # We can specify where in the gripper we want to grasp. About halfway is generally good for + # small objects like this. For a bigger object like a shoe, 0 is better (use the entire + # gripper) + grasp.grasp_params.grasp_palm_to_fingertip = 0.6 + + # The axis on the gripper is the x-axis. + axis_on_gripper_ewrt_gripper = geometry_pb2.Vec3(x=1, y=0, z=0) + + # The axis in the vision frame is the negative z-axis + axis_to_align_with_ewrt_vision = geometry_pb2.Vec3(x=0, y=0, z=-1) + + # Add the vector constraint to our proto. + constraint = grasp.grasp_params.allowable_orientation.add() + constraint.vector_alignment_with_tolerance.axis_on_gripper_ewrt_gripper.CopyFrom( + axis_on_gripper_ewrt_gripper) + constraint.vector_alignment_with_tolerance.axis_to_align_with_ewrt_frame.CopyFrom( + axis_to_align_with_ewrt_vision) + + # We'll take anything within about 15 degrees for top-down or horizontal grasps. + constraint.vector_alignment_with_tolerance.threshold_radians = 0.25 + grasp.grasp_params.grasp_params_frame_name = VISION_FRAME_NAME + + # Build the proto + grasp_request = manipulation_api_pb2.ManipulationApiRequest( + pick_object_in_image=grasp) + + + + #check for success of execution + wait for 10 seconds after request + success = False; start = time.time() + + # Send the request + print('Sending grasp request...') + cmd_response = manipulation_api_client.manipulation_api_command( + manipulation_api_request=grasp_request) + + + + while True: + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_response.manipulation_cmd_id) + + # Send the request + response = manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request) + + print('Current state: ', + manipulation_api_pb2.ManipulationFeedbackState.Name(response.current_state)) + + if response.current_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED or response.current_state == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED: + + if response.current_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED: + success = True + break + + time.sleep(0.25) + + + time.sleep(4.0) + + return success + + + +def arm_object_grasp(robot_state_client, manipulation_api_client, pick_pixels,image): + pick_vec = geometry_pb2.Vec2(x=pick_pixels[0], y=pick_pixels[1]) + + # Build the proto + grasp = manipulation_api_pb2.PickObjectInImage( + pixel_xy=pick_vec, transforms_snapshot_for_camera=image.shot.transforms_snapshot, + frame_name_image_sensor=image.shot.frame_name_image_sensor, + camera_model=image.source.pinhole) + + # We can specify where in the gripper we want to grasp. About halfway is generally good for + # small objects like this. For a bigger object like a shoe, 0 is better (use the entire gripper) + grasp.grasp_params.grasp_palm_to_fingertip = 0.6 + + # Optionally add a grasp constraint. This lets you tell the robot you only want top-down grasps or side-on grasps. + add_grasp_constraint(grasp, robot_state_client) + + # Ask the robot to pick up the object + grasp_request = manipulation_api_pb2.ManipulationApiRequest(pick_object_in_image=grasp) + + # Send the request + cmd_response = manipulation_api_client.manipulation_api_command( + manipulation_api_request=grasp_request) + + # Get feedback from the robot + while True: + feedback_request = manipulation_api_pb2.ManipulationApiFeedbackRequest( + manipulation_cmd_id=cmd_response.manipulation_cmd_id) + + # Send the request + response = manipulation_api_client.manipulation_api_feedback_command( + manipulation_api_feedback_request=feedback_request) + + print('Current state: ', + manipulation_api_pb2.ManipulationFeedbackState.Name(response.current_state)) + + if response.current_state == manipulation_api_pb2.MANIP_STATE_GRASP_SUCCEEDED or response.current_state == manipulation_api_pb2.MANIP_STATE_GRASP_FAILED: + break + + time.sleep(0.25) + + + time.sleep(4.0) + +def add_grasp_constraint(grasp, robot_state_client): + # There are 3 types of constraints: + # 1. Vector alignment + # 2. Full rotation + # 3. Squeeze grasp + # + # You can specify more than one if you want and they will be OR'ed together. + + # For these options, we'll use a vector alignment constraint. + use_vector_constraint = False + force_45_angle_grasp = False + force_squeeze_grasp = False + + # Specify the frame we're using. + grasp.grasp_params.grasp_params_frame_name = VISION_FRAME_NAME + + if use_vector_constraint: + if config.force_top_down_grasp: + # Add a constraint that requests that the x-axis of the gripper is pointing in the + # negative-z direction in the vision frame. + + # The axis on the gripper is the x-axis. + axis_on_gripper_ewrt_gripper = geometry_pb2.Vec3(x=1, y=0, z=0) + + # The axis in the vision frame is the negative z-axis + axis_to_align_with_ewrt_vo = geometry_pb2.Vec3(x=0, y=0, z=-1) + + if config.force_horizontal_grasp: + # Add a constraint that requests that the y-axis of the gripper is pointing in the + # positive-z direction in the vision frame. That means that the gripper is constrained to be rolled 90 degrees and pointed at the horizon. + + # The axis on the gripper is the y-axis. + axis_on_gripper_ewrt_gripper = geometry_pb2.Vec3(x=0, y=1, z=0) + + # The axis in the vision frame is the positive z-axis + axis_to_align_with_ewrt_vo = geometry_pb2.Vec3(x=0, y=0, z=1) + + # Add the vector constraint to our proto. + constraint = grasp.grasp_params.allowable_orientation.add() + constraint.vector_alignment_with_tolerance.axis_on_gripper_ewrt_gripper.CopyFrom( + axis_on_gripper_ewrt_gripper) + constraint.vector_alignment_with_tolerance.axis_to_align_with_ewrt_frame.CopyFrom( + axis_to_align_with_ewrt_vo) + + # We'll take anything within about 10 degrees for top-down or horizontal grasps. + constraint.vector_alignment_with_tolerance.threshold_radians = 0.17 + + elif force_45_angle_grasp: + # Demonstration of a RotationWithTolerance constraint. This constraint allows you to + # specify a full orientation you want the hand to be in, along with a threshold. + # + # You might want this feature when grasping an object with known geometry and you want to + # make sure you grasp a specific part of it. + # + # Here, since we don't have anything in particular we want to grasp, we'll specify an + # orientation that will have the hand aligned with robot and rotated down 45 degrees as an + # example. + + # First, get the robot's position in the world. + robot_state = robot_state_client.get_robot_state() + vision_T_body = get_vision_tform_body(robot_state.kinematic_state.transforms_snapshot) + + # Rotation from the body to our desired grasp. + body_Q_grasp = math_helpers.Quat.from_pitch(0.785398) # 45 degrees + vision_Q_grasp = vision_T_body.rotation * body_Q_grasp + + # Turn into a proto + constraint = grasp.grasp_params.allowable_orientation.add() + constraint.rotation_with_tolerance.rotation_ewrt_frame.CopyFrom(vision_Q_grasp.to_proto()) + + # We'll accept anything within +/- 10 degrees + constraint.rotation_with_tolerance.threshold_radians = 0.17 + + elif force_squeeze_grasp: + # Tell the robot to just squeeze on the ground at the given point. + constraint = grasp.grasp_params.allowable_orientation.add() + constraint.squeeze_grasp.SetInParent() + + +def pixel_to_vision_frame_depth_provided(i,j,depth,rotation_matrix,position): + ''' + Converts a pixel (i,j) in HxW image to 3d position in vision frame + + i,j: pixel location in image + depth_img: HxW depth image + rotaton_matrix: 3x3 rotation matrix of hand in vision frame + position: 3x1 position vector of hand in vision frame + ''' + + #hand_tform_camera comes from line below, just a hardcoded version of it + #rot2 = mesh_frame.get_rotation_matrix_from_xyz((0, np.pi/2, -np.pi/2)) + hand_tform_camera = np.array([[ 3.74939946e-33,6.12323400e-17,1.00000000e+00], + [-1.00000000e+00,6.12323400e-17,0.00000000e+00], + [-6.12323400e-17,-1.00000000e+00,6.12323400e-17]]) + + #Intrinsics for RGB hand camera on spot + CX = 320 + CY = 240 + FX= 552.0291012161067 + FY = 552.0291012161067 + + + z_RGB = depth + x_RGB = (j - CX) * z_RGB / FX + y_RGB = (i - CY) * z_RGB / FY + + bad_z = z_RGB == 0 #if z_RGB is 0, the depth was 0, which means we didn't get a real point. x,y,z will just be where robot hand was + + #first apply rot2 to move camera into hand frame, then apply rotation + transform of hand frame in vision frame + transformed_xyz = np.matmul(rotation_matrix,np.matmul(hand_tform_camera,np.array([x_RGB,y_RGB,z_RGB]))) + position + + return(transformed_xyz,bad_z) + + +def pixel_to_vision_frame(i,j,depth_img,rotation_matrix,position): + ''' + Converts a pixel (i,j) in HxW image to 3d position in vision frame + + i,j: pixel location in image + depth_img: HxW depth image + rotaton_matrix: 3x3 rotation matrix of hand in vision frame + position: 3x1 position vector of hand in vision frame + ''' + + #hand_tform_camera comes from line below, just a hardcoded version of it + #rot2 = mesh_frame.get_rotation_matrix_from_xyz((0, np.pi/2, -np.pi/2)) + hand_tform_camera = np.array([[ 3.74939946e-33,6.12323400e-17,1.00000000e+00], + [-1.00000000e+00,6.12323400e-17,0.00000000e+00], + [-6.12323400e-17,-1.00000000e+00,6.12323400e-17]]) + + #Intrinsics for RGB hand camera on spot + CX = 320 + CY = 240 + FX= 552.0291012161067 + FY = 552.0291012161067 + + + z_RGB = depth_img[i,j] + x_RGB = (j - CX) * z_RGB / FX + y_RGB = (i - CY) * z_RGB / FY + + bad_z = z_RGB == 0 #if z_RGB is 0, the depth was 0, which means we didn't get a real point. x,y,z will just be where robot hand was + + #first apply rot2 to move camera into hand frame, then apply rotation + transform of hand frame in vision frame + transformed_xyz = np.matmul(rotation_matrix,np.matmul(hand_tform_camera,np.array([x_RGB,y_RGB,z_RGB]))) + position + + return(transformed_xyz,bad_z) diff --git a/spot/spot_utils/visualize_data.py b/spot/spot_utils/visualize_data.py new file mode 100644 index 00000000..4f2cdc0a --- /dev/null +++ b/spot/spot_utils/visualize_data.py @@ -0,0 +1,151 @@ +import pickle +import matplotlib.pyplot as plt +import math +import numpy as np +import os +import cv2 +import open3d as o3d + +viz_poses = True + +dir_name = "spot-depth-color-pose-data2/" +dir_path = "../data/" + +pose_data_fname = "pose_data.pkl" +pose_dir = pickle.load(open(dir_path+dir_name+pose_data_fname,"rb")) + +####################################### +# Graph robot poses +if viz_poses: + x_positions = [] + y_positions = [] + x_rots = [] + y_rots = [] + labels = [] + for (counter,pose) in pose_dir.items(): + x_pos = pose['position'][0] + y_pos = pose['position'][1] + yaw = pose['rpy'][2] #euler angles in radians + x_rot = math.cos(yaw) + y_rot = math.sin(yaw) + + + labels.append(counter) + x_positions.append(x_pos) + y_positions.append(y_pos) + x_rots.append(x_rot) + y_rots.append(y_rot) + + plt.scatter(x_positions,y_positions) + plt.quiver(x_positions,y_positions,x_rots,y_rots) + for i, label in enumerate(labels): + plt.annotate(label, (x_positions[i], y_positions[i])) + plt.show() + +####################################### +# Visualize point cloud +CX = 320 +CY = 240 +FX= 552.0291012161067 +FY = 552.0291012161067 + +#rot2_mat comes from rot2 line below, just a hardcoded version of it, it's the rotation from camera to hand frame +#rot2 = mesh_frame.get_rotation_matrix_from_xyz((0, np.pi/2, -np.pi/2)) +rot2_mat = np.array([[ 3.74939946e-33,6.12323400e-17,1.00000000e+00], + [-1.00000000e+00,6.12323400e-17,0.00000000e+00], + [-6.12323400e-17,-1.00000000e+00,6.12323400e-17]]) + + +R = np.array([[FX, 0 ,CX],[0,FY,CY],[0,0,1]]) +T = np.array([1,1,2]) +file_names = os.listdir(dir_path+dir_name) +num_files = int((len(file_names)-1)/ 3.0) +total_pcds = [] +total_axes = [] +for file_num in range(num_files): + total_colors = [] + + color_img = cv2.imread(dir_path+dir_name+"color_"+str(file_num)+".jpg") + color_img = color_img[:,:,::-1] # RGB-> BGR + depth_img = pickle.load(open(dir_path+dir_name+"depth_"+str(file_num),"rb"))#cv2.imread(dir_path+dir_name+"depth_"+str(file_num)+".jpg") + + print(depth_img.shape) + + H,W = depth_img.shape + print(H,W) + pcd = [] + colors = [] + for i in range(H): + for j in range(W): + + z_RGB = depth_img[i,j] + x_RGB = (j - CX) * z_RGB / FX + y_RGB = (i - CY) * z_RGB / FY + + #first apply rot2 to move camera into hand frame, then apply rotation + transform of hand frame in vision frame + transformed_xyz = np.matmul(pose_dir[file_num]['rotation_matrix'],np.matmul(rot2_mat,np.array([x_RGB,y_RGB,z_RGB]))) + pose_dir[file_num]['position'] + + #print(i,j,depth_img[i,j],z_RGB) + + """ + Convert from rgb camera coordinates system + to rgb image coordinates system: + """ + + if z_RGB != 0: + j_rgb = int((x_RGB * FX) / z_RGB + CX) + i_rgb = int((y_RGB * FY) / z_RGB + CY) + + # Add point to point cloud: + #pcd.append([x_RGB, y_RGB, z_RGB]) + pcd.append(transformed_xyz) + + # Add the color of the pixel if it exists: + if 0 <= j_rgb < W and 0 <= i_rgb < H: + colors.append(color_img[i_rgb,j_rgb] / 255) + else: + colors.append([0., 0., 0.]) + + mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6,origin=[0,0,0]) + mesh_frame = mesh_frame.rotate(rot2_mat,center=(0,0,0)).rotate(pose_dir[file_num]['rotation_matrix'], center=(0, 0, 0)).translate(pose_dir[file_num]['position']) + #mesh_frame.paint_uniform_color([float(file_num)/num_files, 0.1, 1-(float(file_num)/num_files)]) + + total_axes.append(mesh_frame) + + pcd_o3d = o3d.geometry.PointCloud() # create a point cloud object + pcd_o3d.points = o3d.utility.Vector3dVector(pcd) + pcd_o3d.colors = o3d.utility.Vector3dVector(colors) + + total_pcds.append(pcd_o3d) + # Convert to Open3D.PointCLoud + ''' + pcd_o3d = o3d.geometry.PointCloud() # create a point cloud object + pcd_o3d.points = o3d.utility.Vector3dVector(pcd) + pcd_o3d.colors = o3d.utility.Vector3dVector(colors) + # Visualize: + o3d.visualization.draw_geometries([pcd_o3d,mesh_frame]) + ''' + #Visualize color and depth + + ''' + plt.imshow(depth_img) + plt.show() + + plt.imshow(color_img) + plt.show() + + min_val = np.min(depth_img) + max_val = np.max(depth_img) + depth_range = max_val - min_val + depth8 = (255.0 / depth_range * (depth_img - min_val)).astype('uint8') + plt.imshow(depth8) + plt.show() + ''' + +# Convert to Open3D.PointCLoud: +#pcd_o3d = o3d.geometry.PointCloud() # create a point cloud object +#pcd_o3d.points = o3d.utility.Vector3dVector(total_pcd) +#pcd_o3d.colors = o3d.utility.Vector3dVector(total_colors) +# Visualize: +origin_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1,origin=[0,0,0]).rotate(rot2_mat,center=(0,0,0)) +o3d.visualization.draw_geometries(total_pcds+total_axes+[origin_frame]) \ No newline at end of file diff --git a/spot/stitch_front_images.py b/spot/stitch_front_images.py new file mode 100644 index 00000000..d34536b8 --- /dev/null +++ b/spot/stitch_front_images.py @@ -0,0 +1,369 @@ +# Copyright (c) 2023 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +"""Stitch frontleft_fisheye_image, frontright_fisheye_image from Image Service.""" + +import io +import os +import sys +from contextlib import contextmanager +from ctypes import * + +import numpy +import OpenGL +import pygame +from OpenGL.GL import * +from OpenGL.GL import GL_VERTEX_SHADER, shaders +from OpenGL.GLU import * +from PIL import Image +from pygame.locals import * + +import bosdyn.api +import bosdyn.client.util +from bosdyn.api import image_pb2 +from bosdyn.client.frame_helpers import BODY_FRAME_NAME, get_a_tform_b, get_vision_tform_body +from bosdyn.client.image import ImageClient, build_image_request + + +class ImagePreppedForOpenGL(): + """Prep image for OpenGL from Spot image_response.""" + + def extract_image(self, image_response): + """Return numpy_array of input image_response image.""" + image_format = image_response.shot.image.format + + if image_format == image_pb2.Image.FORMAT_RAW: + raise Exception('Won\'t work. Yet.') + elif image_format == image_pb2.Image.FORMAT_JPEG: + numpy_array = numpy.asarray(Image.open(io.BytesIO(image_response.shot.image.data))) + else: + raise Exception('Won\'t work.') + + return numpy_array + + def __init__(self, image_response): + self.image = self.extract_image(image_response) + self.body_T_image_sensor = get_a_tform_b(image_response.shot.transforms_snapshot, \ + BODY_FRAME_NAME, image_response.shot.frame_name_image_sensor) + self.vision_T_body = get_vision_tform_body(image_response.shot.transforms_snapshot) + if not self.body_T_image_sensor: + raise Exception('Won\'t work.') + + if image_response.source.pinhole: + resolution = numpy.asarray([ \ + image_response.source.cols, \ + image_response.source.rows]) + + focal_length = numpy.asarray([ \ + image_response.source.pinhole.intrinsics.focal_length.x, \ + image_response.source.pinhole.intrinsics.focal_length.y]) + + principal_point = numpy.asarray([ \ + image_response.source.pinhole.intrinsics.principal_point.x, \ + image_response.source.pinhole.intrinsics.principal_point.y]) + else: + raise Exception('Won\'t work.') + + sensor_T_vo = (self.vision_T_body * self.body_T_image_sensor).inverse() + + camera_projection_mat = numpy.eye(4) + camera_projection_mat[0, 0] = (focal_length[0] / resolution[0]) + camera_projection_mat[0, 2] = (principal_point[0] / resolution[0]) + camera_projection_mat[1, 1] = (focal_length[1] / resolution[1]) + camera_projection_mat[1, 2] = (principal_point[1] / resolution[1]) + + self.MVP = camera_projection_mat.dot(sensor_T_vo.to_matrix()) + + +class ImageInsideOpenGL(): + """Create OpenGL Texture""" + + def __init__(self, numpy_array): + glEnable(GL_TEXTURE_2D) + self.pointer = glGenTextures(1) + with self.manage_bind(): + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, numpy_array.shape[1], numpy_array.shape[0], 0, \ + GL_LUMINANCE, GL_UNSIGNED_BYTE, numpy_array) + glTexParameter(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR) + glTexParameter(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR) + + @contextmanager + def manage_bind(self): + glBindTexture(GL_TEXTURE_2D, self.pointer) # bind image + try: + yield + finally: + glBindTexture(GL_TEXTURE_2D, 0) # unbind image + + def update(self, numpy_array): + """Update texture""" + with self.manage_bind(): + glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, numpy_array.shape[1], numpy_array.shape[0], \ + GL_LUMINANCE, GL_UNSIGNED_BYTE, numpy_array) + + +class StitchingCamera(object): + """Camera to render from in OpenGL.""" + + def __init__(self, image_1, image_2): + """We assume the two images passed in are Front Right and Front Left, + we put our fake OpenGl rendering camera smack dab in the middle of the + two""" + super(StitchingCamera, self).__init__() + + rect_stitching_distance_meters = 2.0 + + vo_T_body = image_1.vision_T_body.to_matrix() + + eye_wrt_body = proto_vec_T_numpy(image_1.body_T_image_sensor.position) \ + + proto_vec_T_numpy(image_2.body_T_image_sensor.position) + + # Add the two real camera norms together to get the fake camera norm. + eye_norm_wrt_body = numpy.array(image_1.body_T_image_sensor.rot.transform_point(0, 0, 1)) \ + + numpy.array(image_2.body_T_image_sensor.rot.transform_point(0, 0, 1)) + + # Make the virtual camera centered. + eye_wrt_body[1] = 0 + eye_norm_wrt_body[1] = 0 + + # Make sure our normal has length 1 + eye_norm_wrt_body = normalize(eye_norm_wrt_body) + + plane_wrt_body = eye_wrt_body + eye_norm_wrt_body * rect_stitching_distance_meters + + self.plane_wrt_vo = mat4mul3(vo_T_body, plane_wrt_body) + self.plane_norm_wrt_vo = mat4mul3(vo_T_body, eye_norm_wrt_body, 0) + + self.eye_wrt_vo = mat4mul3(vo_T_body, eye_wrt_body) + self.up_wrt_vo = mat4mul3(vo_T_body, numpy.array([0, 0, 1]), 0) + + +class CompiledShader(): + """OpenGL shader compile""" + + def __init__(self, vert_shader, frag_shader): + self.program = shaders.compileProgram( \ + shaders.compileShader(vert_shader, GL_VERTEX_SHADER), \ + shaders.compileShader(frag_shader, GL_FRAGMENT_SHADER) \ + ) + self.camera1_MVP = glGetUniformLocation(self.program, 'camera1_MVP') + self.camera2_MVP = glGetUniformLocation(self.program, 'camera2_MVP') + + self.image1_texture = glGetUniformLocation(self.program, 'image1') + self.image2_texture = glGetUniformLocation(self.program, 'image2') + + self.initialized = False + self.image1 = None + self.image2 = None + self.matrix1 = None + self.matrix2 = None + + def update_images(self, image_1, image_2): + self.initialized = True + self.matrix1 = image_1.MVP + self.matrix2 = image_2.MVP + if self.image1 is None: + self.image1 = ImageInsideOpenGL(image_1.image) + else: + self.image1.update(image_1.image) + if self.image2 is None: + self.image2 = ImageInsideOpenGL(image_2.image) + else: + self.image2.update(image_2.image) + + +def load_get_image_response_from_binary_file(file_path): + """Read in image from image response""" + if not os.path.exists(file_path): + raise IOError(f'File not found at: {file_path}') + + _images = image_pb2.GetImageResponse() + with open(file_path, 'rb') as f: + data = f.read() + _images.ParseFromString(data) + + return _images + + +def proto_vec_T_numpy(vec): + return numpy.array([vec.x, vec.y, vec.z]) + + +def mat4mul3(mat, vec, vec4=1): + ret = numpy.matmul(mat, numpy.append(vec, vec4)) + return ret[:-1] + + +def normalize(vec): + norm = numpy.linalg.norm(vec) + if norm == 0: + raise ValueError('norm function returned 0.') + return vec / norm + + +def draw_geometry(plane_wrt_vo, plane_norm_wrt_vo, sz_meters): + """Draw as GL_TRIANGLES.""" + plane_left_wrt_vo = normalize(numpy.cross(numpy.array([0, 0, 1]), plane_norm_wrt_vo)) + if plane_left_wrt_vo is None: + return + plane_up_wrt_vo = normalize(numpy.cross(plane_norm_wrt_vo, plane_left_wrt_vo)) + if plane_up_wrt_vo is None: + return + + plane_up_wrt_vo = plane_up_wrt_vo * sz_meters + plane_left_wrt_vo = plane_left_wrt_vo * sz_meters + + vertices = ( + plane_wrt_vo + plane_left_wrt_vo - plane_up_wrt_vo, + plane_wrt_vo + plane_left_wrt_vo + plane_up_wrt_vo, + plane_wrt_vo - plane_left_wrt_vo + plane_up_wrt_vo, + plane_wrt_vo - plane_left_wrt_vo - plane_up_wrt_vo, + ) + + indices = (0, 1, 2, 0, 2, 3) + + glBegin(GL_TRIANGLES) + for index in indices: + glVertex3fv(vertices[index]) + glEnd() + + +def draw_routine(display, program, stitching_camera): + """OpenGL Draw""" + + glMatrixMode(GL_PROJECTION) + glLoadIdentity() + gluPerspective(110, (display[0] / display[1]), 0.1, 50.0) + + if not program.initialized: + print('Gl is not ready yet.') + return + if stitching_camera is None: + print('No stitching camera yet.') + return + + glUseProgram(program.program) + + glActiveTexture(GL_TEXTURE0 + 0) + with program.image1.manage_bind(): + glUniform1i(program.image1_texture, 0) + glActiveTexture(GL_TEXTURE0 + 1) + + with program.image2.manage_bind(): + + glUniform1i(program.image2_texture, 1) + + glUniformMatrix4fv(program.camera1_MVP, 1, GL_TRUE, program.matrix1) + glUniformMatrix4fv(program.camera2_MVP, 1, GL_TRUE, program.matrix2) + + plane_wrt_vo = stitching_camera.plane_wrt_vo + plane_norm_wrt_vo = stitching_camera.plane_norm_wrt_vo + eye_wrt_vo = stitching_camera.eye_wrt_vo + up_wrt_vo = stitching_camera.up_wrt_vo + breakpoint() + + glMatrixMode(GL_MODELVIEW) + glLoadIdentity() + gluLookAt(eye_wrt_vo[0], eye_wrt_vo[1], eye_wrt_vo[2], \ + plane_wrt_vo[0], plane_wrt_vo[1], plane_wrt_vo[2], \ + up_wrt_vo[0], up_wrt_vo[1], up_wrt_vo[2]) + + rect_sz_meters = 7 + draw_geometry(plane_wrt_vo, plane_norm_wrt_vo, rect_sz_meters) + + +def stitch(robot, options): + """Stitch two front fisheye images together""" + pygame.init() + + display = (1080, 720) + pygame.display.set_mode(display, pygame.DOUBLEBUF | pygame.OPENGL) + clock = pygame.time.Clock() + + with open('shader_vert.glsl', 'r') as file: + vert_shader = file.read() + with open('shader_frag.glsl', 'r') as file: + frag_shader = file.read() + + program = CompiledShader(vert_shader, frag_shader) + + image_client = robot.ensure_client(ImageClient.default_service_name) + + image_sources = ['frontright_fisheye_image', 'frontleft_fisheye_image'] + + requests = [ + build_image_request(source, quality_percent=options.jpeg_quality_percent) + for source in image_sources + ] + + running = True + + images_future = None + stitching_camera = None + + while running: + + display = (1080, 720) + pygame.display.set_mode(display, pygame.DOUBLEBUF | pygame.OPENGL) + + for event in pygame.event.get(): + if event.type == pygame.QUIT: + running = False + + if images_future is not None and images_future.done(): + try: + images = images_future.result() + except Exception as exc: + print('Could not get images:', exc) + else: + for image in images: + if image.source.name == 'frontright_fisheye_image': + front_right = ImagePreppedForOpenGL(image) + elif image.source.name == 'frontleft_fisheye_image': + front_left = ImagePreppedForOpenGL(image) + + if front_right is not None and front_left is not None: + program.update_images(front_right, front_left) + stitching_camera = StitchingCamera(front_right, front_left) + else: + print('Got image response, but not with both images!') + + # Reset variable so we re-send next image request + images_future = None + + if images_future is None: + images_future = image_client.get_image_async(requests, timeout=5.0) + + glClearColor(0, 0, 255, 0) + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) + draw_routine(display, program, stitching_camera) + pygame.display.flip() + clock.tick(60) + + +def main(): + """Top-level function to stitch together two Spot front camera images.""" + import argparse + + parser = argparse.ArgumentParser(description=__doc__) + bosdyn.client.util.add_base_arguments(parser) + parser.add_argument('-j', '--jpeg-quality-percent', help='JPEG quality percentage (0-100)', + type=int, default=50) + options = parser.parse_args() + + sdk = bosdyn.client.create_standard_sdk('front_cam_stitch') + robot = sdk.create_robot(options.hostname) + bosdyn.client.util.authenticate(robot) + robot.sync_with_directory() + robot.time_sync.wait_for_sync() + + stitch(robot, options) + + return True + + +if __name__ == '__main__': + main() diff --git a/spot/test.txt b/spot/test.txt new file mode 100644 index 00000000..02dac6af --- /dev/null +++ b/spot/test.txt @@ -0,0 +1 @@ +TypeError: Object of type DoubleValue is not JSON serializable diff --git a/spot/view_map.py b/spot/view_map.py new file mode 100644 index 00000000..32f1ba7c --- /dev/null +++ b/spot/view_map.py @@ -0,0 +1,469 @@ +# Copyright (c) 2022 Boston Dynamics, Inc. All rights reserved. +# +# Downloading, reproducing, distributing or otherwise using the SDK Software +# is subject to the terms and conditions of the Boston Dynamics Software +# Development Kit License (20191101-BDSDK-SL). + +import argparse +import math +import os +import sys +import time + +import google.protobuf.timestamp_pb2 +import numpy as np +import numpy.linalg +import vtk +from vtk.util import numpy_support + +from bosdyn.api import geometry_pb2 +from bosdyn.api.graph_nav import map_pb2 +from bosdyn.client.frame_helpers import * +from bosdyn.client.math_helpers import * + +""" +This example shows how to load and view a graph nav map. + +""" + + +def numpy_to_poly_data(pts): + """ + Converts numpy array data into vtk poly data. + :param pts: the numpy array to convert (3 x N). + :return: a vtkPolyData. + """ + pd = vtk.vtkPolyData() + pd.SetPoints(vtk.vtkPoints()) + # Makes a deep copy + pd.GetPoints().SetData(numpy_support.numpy_to_vtk(pts.copy())) + + f = vtk.vtkVertexGlyphFilter() + f.SetInputData(pd) + f.Update() + pd = f.GetOutput() + + return pd + + +def mat_to_vtk(mat): + """ + Converts a 4x4 homogenous transform into a vtk transform object. + :param mat: A 4x4 homogenous transform (numpy array). + :return: A VTK transform object representing the transform. + """ + t = vtk.vtkTransform() + t.SetMatrix(mat.flatten()) + return t + + +def vtk_to_mat(transform): + """ + Converts a VTK transform object to 4x4 homogenous numpy matrix. + :param transform: an object of type vtkTransform + : return: a numpy array with a 4x4 matrix representation of the transform. + """ + tf_matrix = transform.GetMatrix() + out = np.array(np.eye(4)) + for r in range(4): + for c in range(4): + out[r, c] = tf_matrix.GetElement(r, c) + return out + + +def api_to_vtk_se3_pose(se3_pose): + """ + Convert a bosdyn SDK SE3Pose into a VTK pose. + :param se3_pose: the bosdyn SDK SE3 Pose. + :return: A VTK pose representing the bosdyn SDK SE3 Pose. + """ + return mat_to_vtk(se3_pose.to_matrix()) + + +def create_fiducial_object(world_object, waypoint, renderer): + """ + Creates a VTK object representing a fiducial. + :param world_object: A WorldObject representing a fiducial. + :param waypoint: The waypoint the AprilTag is associated with. + :param renderer: The VTK renderer + :return: a tuple of (vtkActor, 4x4 homogenous transform) representing the vtk actor for the fiducial, and its + transform w.r.t the waypoint. + """ + fiducial_object = world_object.apriltag_properties + odom_tform_fiducial_filtered = get_a_tform_b( + world_object.transforms_snapshot, ODOM_FRAME_NAME, + world_object.apriltag_properties.frame_name_fiducial_filtered) + waypoint_tform_odom = SE3Pose.from_obj(waypoint.waypoint_tform_ko) + waypoint_tform_fiducial_filtered = api_to_vtk_se3_pose( + waypoint_tform_odom * odom_tform_fiducial_filtered) + plane_source = vtk.vtkPlaneSource() + plane_source.SetCenter(0.0, 0.0, 0.0) + plane_source.SetNormal(0.0, 0.0, 1.0) + plane_source.Update() + plane = plane_source.GetOutput() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(plane) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetColor(0.5, 0.7, 0.9) + actor.SetScale(fiducial_object.dimensions.x, fiducial_object.dimensions.y, 1.0) + renderer.AddActor(actor) + return actor, waypoint_tform_fiducial_filtered + + +def create_point_cloud_object(waypoints, snapshots, waypoint_id): + """ + Create a VTK object representing the point cloud in a snapshot. Note that in graph_nav, "point cloud" refers to the + feature cloud of a waypoint -- that is, a collection of visual features observed by all five cameras at a particular + point in time. The visual features are associated with points that are rigidly attached to a waypoint. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of waypoint snapshot ID to waypoint snapshot. + :param waypoint_id: the waypoint ID of the waypoint whose point cloud we want to render. + :return: a vtkActor containing the point cloud data. + """ + wp = waypoints[waypoint_id] + snapshot = snapshots[wp.snapshot_id] + cloud = snapshot.point_cloud + odom_tform_cloud = get_a_tform_b(cloud.source.transforms_snapshot, ODOM_FRAME_NAME, + cloud.source.frame_name_sensor) + waypoint_tform_odom = SE3Pose.from_obj(wp.waypoint_tform_ko) + waypoint_tform_cloud = api_to_vtk_se3_pose(waypoint_tform_odom * odom_tform_cloud) + + point_cloud_data = np.frombuffer(cloud.data, dtype=np.float32).reshape(int(cloud.num_points), 3) + poly_data = numpy_to_poly_data(point_cloud_data) + arr = vtk.vtkFloatArray() + for i in range(cloud.num_points): + arr.InsertNextValue(point_cloud_data[i, 2]) + arr.SetName("z_coord") + poly_data.GetPointData().AddArray(arr) + poly_data.GetPointData().SetActiveScalars("z_coord") + actor = vtk.vtkActor() + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputData(poly_data) + mapper.ScalarVisibilityOn() + actor.SetMapper(mapper) + actor.GetProperty().SetPointSize(2) + actor.SetUserTransform(waypoint_tform_cloud) + return actor + + +def create_waypoint_object(renderer, waypoints, snapshots, waypoint_id): + """ + Creates a VTK object representing a waypoint and its point cloud. + :param renderer: The VTK renderer. + :param waypoints: dict of waypoint ID to waypoint. + :param snapshots: dict of snapshot ID to snapshot. + :param waypoint_id: the waypoint id of the waypoint object we wish to create. + :return: A vtkAssembly representing the waypoint (an axis) and its point cloud. + """ + assembly = vtk.vtkAssembly() + actor = vtk.vtkAxesActor() + actor.SetXAxisLabelText("") + actor.SetYAxisLabelText("") + actor.SetZAxisLabelText("") + actor.SetTotalLength(0.2, 0.2, 0.2) + point_cloud_actor = create_point_cloud_object(waypoints, snapshots, waypoint_id) + assembly.AddPart(actor) + assembly.AddPart(point_cloud_actor) + renderer.AddActor(assembly) + return assembly + + +def make_line(pt_A, pt_B, renderer): + """ + Creates a VTK object which is a white line between two points. + :param pt_A: starting point of the line. + :param pt_B: ending point of the line. + :param renderer: the VTK renderer. + :return: A VTK object that is a while line between pt_A and pt_B. + """ + line_source = vtk.vtkLineSource() + line_source.SetPoint1(pt_A[0], pt_A[1], pt_A[2]) + line_source.SetPoint2(pt_B[0], pt_B[1], pt_B[2]) + mapper = vtk.vtkPolyDataMapper() + mapper.SetInputConnection(line_source.GetOutputPort()) + + actor = vtk.vtkActor() + actor.SetMapper(mapper) + actor.GetProperty().SetLineWidth(2) + actor.GetProperty().SetColor(0.7, 0.7, 0.7) + renderer.AddActor(actor) + return actor + + +def make_text(name, pt, renderer): + """ + Creates white text on a black background at a particular point. + :param name: The text to display. + :param pt: The point in the world where the text will be displayed. + :param renderer: The VTK renderer + :return: the vtkActor representing the text. + """ + actor = vtk.vtkTextActor() + actor.SetInput(name) + prop = actor.GetTextProperty() + prop.SetBackgroundColor(0.0, 0.0, 0.0) + prop.SetBackgroundOpacity(0.5) + prop.SetFontSize(16) + coord = actor.GetPositionCoordinate() + coord.SetCoordinateSystemToWorld() + coord.SetValue((pt[0], pt[1], pt[2])) + + renderer.AddActor(actor) + return actor + + +def create_edge_object(curr_wp_tform_to_wp, world_tform_curr_wp, renderer): + # Concatenate the edge transform. + world_tform_to_wp = np.dot(world_tform_curr_wp, curr_wp_tform_to_wp) + # Make a line between the current waypoint and the neighbor. + make_line(world_tform_curr_wp[:3, 3], world_tform_to_wp[:3, 3], renderer) + return world_tform_to_wp + + +def load_map(path): + """ + Load a map from the given file path. + :param path: Path to the root directory of the map. + :return: the graph, waypoints, waypoint snapshots and edge snapshots. + """ + with open(os.path.join(path, "graph"), "rb") as graph_file: + # Load the graph file and deserialize it. The graph file is a protobuf containing only the waypoints and the + # edges between them. + data = graph_file.read() + current_graph = map_pb2.Graph() + current_graph.ParseFromString(data) + + # Set up maps from waypoint ID to waypoints, edges, snapshots, etc. + current_waypoints = {} + current_waypoint_snapshots = {} + current_edge_snapshots = {} + current_anchors = {} + current_anchored_world_objects = {} + + # Load the anchored world objects first so we can look in each waypoint snapshot as we load it. + for anchored_world_object in current_graph.anchoring.objects: + current_anchored_world_objects[anchored_world_object.id] = (anchored_world_object,) + # For each waypoint, load any snapshot associated with it. + for waypoint in current_graph.waypoints: + current_waypoints[waypoint.id] = waypoint + + if len(waypoint.snapshot_id) == 0: + continue + # Load the snapshot. Note that snapshots contain all of the raw data in a waypoint and may be large. + file_name = os.path.join(path, "waypoint_snapshots", waypoint.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + waypoint_snapshot = map_pb2.WaypointSnapshot() + waypoint_snapshot.ParseFromString(snapshot_file.read()) + current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot + + for fiducial in waypoint_snapshot.objects: + if not fiducial.HasField("apriltag_properties"): + continue + + str_id = str(fiducial.apriltag_properties.tag_id) + if (str_id in current_anchored_world_objects and + len(current_anchored_world_objects[str_id]) == 1): + + # Replace the placeholder tuple with a tuple of (wo, waypoint, fiducial). + anchored_wo = current_anchored_world_objects[str_id][0] + current_anchored_world_objects[str_id] = (anchored_wo, waypoint, fiducial) + + # Similarly, edges have snapshot data. + for edge in current_graph.edges: + if len(edge.snapshot_id) == 0: + continue + file_name = os.path.join(path, "edge_snapshots", edge.snapshot_id) + if not os.path.exists(file_name): + continue + with open(file_name, "rb") as snapshot_file: + edge_snapshot = map_pb2.EdgeSnapshot() + edge_snapshot.ParseFromString(snapshot_file.read()) + current_edge_snapshots[edge_snapshot.id] = edge_snapshot + for anchor in current_graph.anchoring.anchors: + current_anchors[anchor.id] = anchor + print("Loaded graph with {} waypoints, {} edges, {} anchors, and {} anchored world objects". + format(len(current_graph.waypoints), len(current_graph.edges), + len(current_graph.anchoring.anchors), len(current_graph.anchoring.objects))) + return (current_graph, current_waypoints, current_waypoint_snapshots, + current_edge_snapshots, current_anchors, current_anchored_world_objects) + + +def create_anchored_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, + current_anchors, current_anchored_world_objects, renderer): + """ + Creates all the VTK objects associated with the graph, in seed frame, if they are anchored. + :param current_graph: the graph to use. + :param current_waypoint_snapshots: dict from snapshot id to snapshot. + :param current_waypoints: dict from waypoint id to waypoint. + :param renderer: The VTK renderer + :return: the average position in world space of all the waypoints. + """ + waypoint_objects = {} + avg_pos = np.array([0.0, 0.0, 0.0]) + waypoints_in_anchoring = 0 + # Create VTK objects associated with each waypoint. + for waypoint in current_graph.waypoints: + if waypoint.id in current_anchors: + waypoint_object = create_waypoint_object(renderer, current_waypoints, + current_waypoint_snapshots, waypoint.id) + seed_tform_waypoint = SE3Pose.from_obj( + current_anchors[waypoint.id].seed_tform_waypoint).to_matrix() + waypoint_object.SetUserTransform(mat_to_vtk(seed_tform_waypoint)) + # make_text(waypoint.annotations.name, seed_tform_waypoint[:3, 3], renderer) + avg_pos += seed_tform_waypoint[:3, 3] + waypoints_in_anchoring += 1 + + avg_pos /= waypoints_in_anchoring + + # Create VTK objects associated with each edge. + for edge in current_graph.edges: + if edge.id.from_waypoint in current_anchors and edge.id.to_waypoint in current_anchors: + seed_tform_from = SE3Pose.from_obj( + current_anchors[edge.id.from_waypoint].seed_tform_waypoint).to_matrix() + from_tform_to = SE3Pose.from_obj(edge.from_tform_to).to_matrix() + create_edge_object(from_tform_to, seed_tform_from, renderer) + + # Create VTK objects associated with each anchored world object. + for anchored_wo in current_anchored_world_objects.values(): + # anchored_wo is a tuple of (anchored_world_object, waypoint, fiducial). + (fiducial_object, _) = create_fiducial_object(anchored_wo[2], anchored_wo[1], renderer) + seed_tform_fiducial = SE3Pose.from_obj(anchored_wo[0].seed_tform_object).to_matrix() + fiducial_object.SetUserTransform(mat_to_vtk(seed_tform_fiducial)) + # make_text(anchored_wo[0].id, seed_tform_fiducial[:3, 3], renderer) + + return avg_pos + + +def create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, renderer): + """ + Creates all the VTK objects associated with the graph. + :param current_graph: the graph to use. + :param current_waypoint_snapshots: dict from snapshot id to snapshot. + :param current_waypoints: dict from waypoint id to waypoint. + :param renderer: The VTK renderer + :return: the average position in world space of all the waypoints. + """ + waypoint_objects = {} + # Create VTK objects associated with each waypoint. + for waypoint in current_graph.waypoints: + waypoint_objects[waypoint.id] = create_waypoint_object(renderer, current_waypoints, + current_waypoint_snapshots, + waypoint.id) + # Now, perform a breadth first search of the graph starting from an arbitrary waypoint. Graph nav graphs + # have no global reference frame. The only thing we can say about waypoints is that they have relative + # transformations to their neighbors via edges. So the goal is to get the whole graph into a global reference + # frame centered on some waypoint as the origin. + queue = [] + queue.append((current_graph.waypoints[0], np.eye(4))) + visited = {} + # Get the camera in the ballpark of the right position by centering it on the average position of a waypoint. + avg_pos = np.array([0.0, 0.0, 0.0]) + + # Breadth first search. + while len(queue) > 0: + # Visit a waypoint. + curr_element = queue[0] + queue.pop(0) + curr_waypoint = curr_element[0] + if curr_waypoint.id in visited: + continue + visited[curr_waypoint.id] = True + + # We now know the global pose of this waypoint, so set the pose. + waypoint_objects[curr_waypoint.id].SetUserTransform(mat_to_vtk(curr_element[1])) + world_tform_current_waypoint = curr_element[1] + # Add text to the waypoint. + # make_text(curr_waypoint.annotations.name, world_tform_current_waypoint[:3, 3], renderer) + + # For each fiducial in the waypoint's snapshot, add an object at the world pose of that fiducial. + if (curr_waypoint.snapshot_id in current_waypoint_snapshots): + snapshot = current_waypoint_snapshots[curr_waypoint.snapshot_id] + for fiducial in snapshot.objects: + if fiducial.HasField("apriltag_properties"): + (fiducial_object, curr_wp_tform_fiducial) = create_fiducial_object( + fiducial, curr_waypoint, renderer) + world_tform_fiducial = np.dot(world_tform_current_waypoint, + vtk_to_mat(curr_wp_tform_fiducial)) + fiducial_object.SetUserTransform(mat_to_vtk(world_tform_fiducial)) + # make_text(str(fiducial.apriltag_properties.tag_id), world_tform_fiducial[:3, 3],renderer) + + # Now, for each edge, walk along the edge and concatenate the transform to the neighbor. + for edge in current_graph.edges: + # If the edge is directed away from us... + if edge.id.from_waypoint == curr_waypoint.id and edge.id.to_waypoint not in visited: + current_waypoint_tform_to_waypoint = SE3Pose.from_obj( + edge.from_tform_to).to_matrix() + world_tform_to_wp = create_edge_object(current_waypoint_tform_to_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.to_waypoint], world_tform_to_wp)) + avg_pos += world_tform_to_wp[:3, 3] + # If the edge is directed toward us... + elif edge.id.to_waypoint == curr_waypoint.id and edge.id.from_waypoint not in visited: + current_waypoint_tform_from_waypoint = (SE3Pose.from_obj( + edge.from_tform_to).inverse()).to_matrix() + world_tform_from_wp = create_edge_object(current_waypoint_tform_from_waypoint, + world_tform_current_waypoint, renderer) + # Add the neighbor to the queue. + queue.append((current_waypoints[edge.id.from_waypoint], world_tform_from_wp)) + avg_pos += world_tform_from_wp[:3, 3] + + # Compute the average waypoint position to place the camera appropriately. + avg_pos /= len(current_waypoints) + return avg_pos + + +def main(argv): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('path', type=str, help='Map to draw.') + parser.add_argument('-a', '--anchoring', action='store_true', + help='Draw the map according to the anchoring (in seed frame).') + options = parser.parse_args(argv) + # Load the map from the given file. + (current_graph, current_waypoints, current_waypoint_snapshots, current_edge_snapshots, + current_anchors, current_anchored_world_objects) = load_map(options.path) + + # Create the renderer. + renderer = vtk.vtkRenderer() + renderer.SetBackground(0.05, 0.1, 0.15) + + if options.anchoring: + if len(current_graph.anchoring.anchors) == 0: + print('No anchors to draw.') + sys.exit(-1) + avg_pos = create_anchored_graph_objects(current_graph, current_waypoint_snapshots, + current_waypoints, current_anchors, + current_anchored_world_objects, renderer) + else: + avg_pos = create_graph_objects(current_graph, current_waypoint_snapshots, current_waypoints, + renderer) + + camera_pos = avg_pos + np.array([-1, 0, 5]) + + camera = renderer.GetActiveCamera() + camera.SetViewUp(0, 0, 1) + camera.SetPosition(camera_pos[0], camera_pos[1], camera_pos[2]) + + # Create the VTK renderer and interactor. + renderWindow = vtk.vtkRenderWindow() + renderWindow.SetWindowName(options.path) + renderWindow.AddRenderer(renderer) + renderWindowInteractor = vtk.vtkRenderWindowInteractor() + renderWindowInteractor.SetRenderWindow(renderWindow) + renderWindow.SetSize(1280, 720) + style = vtk.vtkInteractorStyleTerrain() + renderWindowInteractor.SetInteractorStyle(style) + renderer.ResetCamera() + + # Start rendering. + renderWindow.Render() + renderWindow.Start() + renderWindowInteractor.Start() + + +if __name__ == '__main__': + main(sys.argv[1:]) diff --git a/spot/visualize_navmap.py b/spot/visualize_navmap.py new file mode 100644 index 00000000..ef22bf2c --- /dev/null +++ b/spot/visualize_navmap.py @@ -0,0 +1,85 @@ +import networkx as nx +import matplotlib.pyplot as plt +from bosdyn.api.graph_nav import graph_nav_pb2, map_pb2 +import numpy as np +import transformations as tf +import pdb +import os + +graph_fpath = "Trajectories/graphs/upstairs" +SAVE_NAME = 'upstairs.png' + +with open(graph_fpath + '/graph', 'rb') as graph_file: + # Load the graph from disk. + data = graph_file.read() + graph = map_pb2.Graph() + graph.ParseFromString(data) + print( + f'Loaded graph has {len(graph.waypoints)} waypoints and {len(graph.edges)} edges' + ) + +# pdb.set_trace() +objs = [waypoint for waypoint in graph.waypoints] +id2loc = {obj.id: [obj.seed_tform_waypoint.position.x, obj.seed_tform_waypoint.position.y, obj.seed_tform_waypoint.position.z] for obj in graph.anchoring.anchors} +obj2id = {obj.annotations.name: obj.id for obj in objs} +id2obj = {obj.id: obj.annotations.name for obj in objs} + + +max_z = float('-inf'); min_z = float('inf') +for obj in graph.anchoring.anchors: + + max_z = max(obj.seed_tform_waypoint.position.z, max_z) + min_z = min(obj.seed_tform_waypoint.position.z, min_z) + +print('MAX: ', max_z, 'MIN: ', min_z) +map_2d = False + +if map_2d: + # convert spot nva graph to nx graph for planning + connect_graph = nx.Graph() + + nodes = {id: (id2loc[id][0], id2loc[id][1]) for id in id2loc.keys()} + connect_graph.add_nodes_from(nodes) + + connect_graph.add_edges_from([(e.id.from_waypoint, e.id.to_waypoint) for e in graph.edges]) + # node_positions = {node: nodes[node]['pos'] for node, data in connect_graph.nodes(data=True)} + node_positions = nodes + # pdb.set_trace() + fig, ax = plt.subplots() + nx.draw_networkx_nodes(connect_graph, node_positions, ax=ax, node_size=200, node_color='skyblue') + + # Draw edges + nx.draw_networkx_edges(connect_graph, node_positions, ax=ax, width=2, edge_color='gray') +else: + nodes = {id: [id2loc[id][0], id2loc[id][1], id2loc[id][2]] for id in id2loc.keys()} + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + + # Plot the nodes - alpha is scaled by "depth" automatically + ax.scatter(xs= [nodes[x][0] for x in nodes.keys()], ys = [nodes[x][1] for x in nodes.keys()], zs = [nodes[x][2] for x in nodes.keys()], s=20, ec="w", c='skyblue') + + edges = np.array([(nodes[e.id.from_waypoint], nodes[e.id.to_waypoint]) for e in graph.edges]) + + # Plot the edges + for e in edges: + ax.plot(*e.T, color="tab:gray") + +# Add labels to nodes +# traj = ['pebbly-moa-Lk1eKDsWctdahyFUYmuOwQ==', 'apodal-guppy-GiKnSqr5tjLg0mxJrl3D3A==', 'spayed-chunga-mzIz4YM6uDcOTQs4BE6Hog==', 'tensed-boa-xYAA6hlK03sNoyCFv9myLg==', 'barky-lion-CnIv1DhdcBLYaykAMePqYQ==', 'yucky-biped-P4EZeKZ83gNVAM6NVUilwQ==', 'chafed-goose-9EUBS5JEIqzvEADiAaBruA==', 'moldy-cicada-vvOonrkUh3zeP.FvnbpGMQ==', 'fogged-chimp-SciMoeEkEKkwzELllK1GVw==', 'bulgy-ermine-eevg2YudQjiYiB4j4PmfnA==', 'meager-sponge-QNVwDAR+yBl6kFr0xbVQaA==', 'surly-eel-L3WHxzLsncHzXd.VbkuLdw==', 'finer-weasel-1beejRvUz0yBJASf7zbPaQ==', 'gummed-oryx-guW37qvkWHzT9aXma7CxZw==', 'peeved-embryo-Umsv5ZvvrktexhGh7RJpsw==', 'fly-jackal-YH7k7fOllCW3qBmzZLIJmQ==']# labels = {node: id2obj[node] for node in connect_graph.nodes() if node in obj2id.values()} +# pdb.set_trace() +# labels = {node: id2obj[node] for node in connect_graph.nodes() if node in obj2id.values()} +# labels.update({node: '0' for node in connect_graph.nodes() if node in traj}) +# nx.draw_networkx_labels(connect_graph, node_positions, labels=labels, font_size=10, font_color='black') + +# Set plot limits to ensure all nodes are visible +# ax.set_xlim(0, 8) +# ax.set_ylim(-5, 14) +# breakpoint() +# Show the plot +plt.axis('off') # Turn off axis labels and ticks +plt.savefig(os.path.join('.',SAVE_NAME)) +plt.show() + + + +