def saveCalibrationData(self, filename=None):
if filename is None:
filename = os.path.join(spartanUtils.getSpartanSourceDir(),
'sandbox',
'hand_eye_calibration_robot_data.yaml')
spartanUtils.saveToYaml(self.calibrationData, filename)
def downsample_by_pose_difference_threshold(images_dir_full_path, threshold):
pose_yaml = os.path.join(images_dir_full_path, "pose_data.yaml")
pose_dict = spartanUtils.getDictFromYamlFilename(pose_yaml)
images_dir_temp_path = os.path.join(os.path.dirname(images_dir_full_path), 'images_temp')
if not os.path.isdir(images_dir_temp_path):
os.makedirs(images_dir_temp_path)
previous_pose = FusionServer.get_numpy_position_from_pose(pose_dict[0])
print "Using downsampling by pose difference threshold... "
print "Previously: ", len(pose_dict), " images"
num_kept_images = 0
num_deleted_images = 0
for i in range(0,len(pose_dict)):
single_frame_data = pose_dict[i]
this_pose = FusionServer.get_numpy_position_from_pose(pose_dict[i])
if i == 0:
keep_image = True
num_kept_images += 1
elif np.linalg.norm(previous_pose - this_pose) > threshold:
previous_pose = this_pose
num_kept_images += 1
else:
# delete pose from forward kinematics
del pose_dict[i]
continue
# if we have gotten here, then move the images over to the new directory
rgb_filename = os.path.join(images_dir_full_path, single_frame_data['rgb_image_filename'])
rgb_filename_temp = os.path.join(images_dir_temp_path, single_frame_data['rgb_image_filename'])
shutil.move(rgb_filename, rgb_filename_temp)
depth_filename = os.path.join(images_dir_full_path, single_frame_data['depth_image_filename'])
depth_filename_temp = os.path.join(images_dir_temp_path, single_frame_data['depth_image_filename'])
shutil.move(depth_filename, depth_filename_temp)
# # delete pose from posegraph
# del posegraph_list[i-num_deleted_images]
# num_deleted_images += 1
# write downsamples pose_data.yaml (forward kinematics)
spartanUtils.saveToYaml(pose_dict, os.path.join(images_dir_temp_path,'pose_data.yaml'))
# remove old images
shutil.move(os.path.join(images_dir_full_path, 'camera_info.yaml'), os.path.join(images_dir_temp_path, 'camera_info.yaml'))
shutil.rmtree(images_dir_full_path)
print "renaming %s to %s " %(images_dir_temp_path, images_dir_full_path)
# rename temp images to images
os.rename(images_dir_temp_path, images_dir_full_path)
print "After: ", num_kept_images, " images"
def extract_intrinsics(path_to_tar_file, path_to_camera_info_yaml_file,
camera_name):
cmd = 'mkdir -p /tmp/calibrationdata'
os.system(cmd)
cmd = 'tar xvf ' + path_to_tar_file + ' -C ' + '/tmp/calibrationdata' + ' > /dev/null 2>&1'
os.system(cmd)
camera_info_dict = spartanUtils.getDictFromYamlFilename(
'/tmp/calibrationdata/ost.yaml')
print camera_info_dict
camera_info_dict['camera_name'] = camera_name
spartanUtils.saveToYaml(camera_info_dict, path_to_camera_info_yaml_file)
def extract_and_fuse_single_scene(
log_full_path,
downsample=True,
linear_distance_threshold=LINEAR_DISTANCE_THRESHOLD,
angle_distance_threshold=ANGLE_DISTANCE_THRESHOLD):
"""
Extracts all the images from the rosbag.
:param log_full_path:
:param downsample: whether or not to downsample
:param linear_distance_threshold: threshold used in downsampling
:param angle_distance_threshold: threshold used in downsampling
:return:
"""
global fs
if fs is None:
fs = FusionServer()
print "extracting and fusing scene:", log_full_path
log = os.path.split(log_full_path)[-1]
mem()
processed_dir = os.path.join(log_full_path, 'processed')
images_dir = os.path.join(processed_dir, 'images')
raw_dir = os.path.join(log_full_path, 'raw')
raw_close_up_dir = os.path.join(log_full_path, 'raw_close_up')
bag_filepath = os.path.join(log_full_path, 'raw', 'fusion_' + log + '.bag')
bag_close_up_filepath = os.path.join(raw_close_up_dir,
'fusion_' + log + '.bag')
pose_data_filename = os.path.join(images_dir, 'pose_data.yaml')
metadata_filename = os.path.join(images_dir, 'metadata.yaml')
if not already_extracted_rosbag(log_full_path):
print "extracting", bag_filepath
# run this memory intensive step in a process, to allow
# it to release memory back to the operating system
# when finished
proc = mp.Process(target=extract_data_from_rosbag,
args=(bag_filepath, images_dir))
proc.start()
proc.join()
print "finished extracting", bag_filepath
# counter_new_extracted += 1
else:
print "already extracted", bag_filepath
# we need to free some memory
gc.collect()
mem()
if not already_ran_tsdf_fusion(log_full_path):
print "preparing for tsdf_fusion for", log_full_path
tsdf_fusion.format_data_for_tsdf(images_dir)
gc.collect()
print "running tsdf fusion"
fusion_params = FUSION_CONFIG[FUSION_LOCATION]
tsdf_fusion.run_tsdf_fusion_cuda(
images_dir,
bbox_min=fusion_params['bbox_min'],
bbox_max=fusion_params['bbox_max'],
voxel_size=fusion_params['voxel_size'])
gc.collect()
print "converting tsdf to ply"
tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin')
tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply')
tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename)
# counter_new_fused += 1
else:
print "already ran tsdf_fusion for", log_full_path
if not already_downsampled(log_full_path) and downsample:
print "downsampling image folder"
# fs = FusionServer()
fs.downsample_by_pose_difference_threshold(images_dir,
linear_distance_threshold,
angle_distance_threshold)
# counter_new_downsampled += 1
# create metadata.yaml file if it doesn't yet exist
if not os.path.exists(metadata_filename):
pose_data = spartanUtils.getDictFromYamlFilename(
pose_data_filename)
original_image_indices = pose_data.keys()
metadata = dict()
metadata['original_image_indices'] = original_image_indices
metadata['close_up_image_indices'] = []
spartanUtils.saveToYaml(metadata, metadata_filename)
else:
print "already downsampled for", log_full_path
# extract the up close up log if it exists
if os.path.exists(
bag_close_up_filepath
) and not already_extracted_close_up_log(metadata_filename):
print "\n\n-----------extracting close up images----------\n\n"
print "extracting", bag_close_up_filepath
close_up_temp_dir = os.path.join(processed_dir, 'images_close_up')
# run this memory intensive step in a process, to allow
# it to release memory back to the operating system
# when finished
proc = mp.Process(target=extract_data_from_rosbag,
args=(bag_close_up_filepath, close_up_temp_dir))
proc.start()
proc.join()
print "downsampling image folder %s" % (close_up_temp_dir)
if downsample:
fs.downsample_by_pose_difference_threshold(
close_up_temp_dir, linear_distance_threshold,
angle_distance_threshold)
pose_data_close_up_filename = os.path.join(close_up_temp_dir,
'pose_data.yaml')
pose_data_close_up = spartanUtils.getDictFromYamlFilename(
pose_data_close_up_filename)
pose_data = spartanUtils.getDictFromYamlFilename(pose_data_filename)
original_image_indices = pose_data.keys()
largest_original_image_idx = max(original_image_indices)
close_up_image_indices = []
print "largest_original_image_idx", largest_original_image_idx
for img_idx, data in pose_data_close_up.iteritems():
# just to give some buffer and avoid edge cases in some checks
img_idx_new = img_idx + largest_original_image_idx + 4
rgb_filename = "%06i_%s.png" % (img_idx_new, "rgb")
depth_filename = "%06i_%s.png" % (img_idx_new, "depth")
rgb_filename_prev = data['rgb_image_filename']
depth_filename_prev = data['depth_image_filename']
data['rgb_image_filename'] = rgb_filename
data['depth_image_filename'] = depth_filename
pose_data[img_idx_new] = data
close_up_image_indices.append(img_idx_new)
# move images around
shutil.move(os.path.join(close_up_temp_dir, rgb_filename_prev),
os.path.join(images_dir, rgb_filename))
shutil.move(os.path.join(close_up_temp_dir, depth_filename_prev),
os.path.join(images_dir, depth_filename))
shutil.rmtree(close_up_temp_dir)
original_image_indices.sort()
close_up_image_indices.sort()
spartanUtils.saveToYaml(pose_data, pose_data_filename)
metadata = dict()
metadata['normal_image_indices'] = original_image_indices
metadata['close_up_image_indices'] = close_up_image_indices
spartanUtils.saveToYaml(metadata, metadata_filename)
else:
if os.path.exists(bag_close_up_filepath):
print "already extracted close up log, skipping"
else:
print "raw_close_up doesn't exist, skipping"
def processCalibrationData(posegraph_file, robot_data_filename,
save_data_filename):
cameraPoses = CameraPoses(posegraph_file)
calibrationData = spartanUtils.getDictFromYamlFilename(robot_data_filename)
calibrationData = addCameraPosesToDataFile(calibrationData, cameraPoses)
spartanUtils.saveToYaml(calibrationData, save_data_filename)
def runROSCalibration(self, headerData):
headerData['target'][
'location_estimate_in_robot_base_frame'] = self.calibrationPosesConfig[
'target_location']
calibrationRunData = dict()
calibrationRunData['header'] = headerData
#setup our passive subscribers to the IR or RGB data
topicName = None
self.passiveSubscriber = None
if self.captureRGB:
topicName = self.config['rgb_raw_topic']
if self.captureIR:
topicName = self.config['ir_raw_topic']
if topicName:
self.passiveSubscriber = spartanROSUtils.SimpleSubscriber(
topicName, sensor_msgs.msg.Image)
self.passiveSubscriber.start()
unique_name = time.strftime(
"%Y%m%d-%H%M%S") + "_" + self.calibrationType
self.calibrationFolderName = os.path.join(
spartanUtils.getSpartanSourceDir(), 'calibration_data',
unique_name)
os.system("mkdir -p " + self.calibrationFolderName)
os.chdir(self.calibrationFolderName)
poseDict = self.computeCalibrationPoses()
self.drawResult(poseDict)
rospy.loginfo("finished making calibration poses")
rospy.loginfo("starting calibration run")
calibrationData = []
# topic = self.config['rgb_raw_topic']
# msgType = sensor_msgs.msg.Image
# self.subscribers = dict()
# self.subscribers[topic] = spartanROSUtils.SimpleSubscriber(topic, msgType)
# self.subscribers[topic].start()
for pose in poseDict['feasiblePoses']:
# move robot to that joint position
rospy.loginfo("\n moving to pose")
self.robotService.moveToJointPosition(pose['joint_angles'])
rospy.loginfo("capturing images and robot data")
data = self.captureCurrentRobotAndImageData(
captureRGB=self.captureRGB, captureIR=self.captureIR)
calibrationData.append(data)
rospy.loginfo("finished calibration routine, saving data to file")
calibrationRunData['data_list'] = calibrationData
spartanUtils.saveToYaml(
calibrationRunData,
os.path.join(self.calibrationFolderName, 'robot_data.yaml'))
if self.passiveSubscriber:
self.passiveSubscriber.stop()
return calibrationRunData
data_in = json.load(open(stored_poses_json_source))
#pprint(data_in)
stored_poses_yaml_destination = os.path.join(
spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects',
'station_config', 'RLG_iiwa_1', 'stored_poses.yaml')
data_out = dict()
joint_names = [
'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4',
'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'
]
data_out['header'] = dict()
data_out['header']['robot'] = 'kuka_iiwa'
data_out['header']['num_joints'] = 7
data_out['header']['joint_names'] = joint_names
for group in data_in:
data_out[str(group)] = dict()
print "Group is", group
for pose in data_in[group]:
pose_name = pose['name']
print "pose_name is", pose_name
data_out[str(group)][str(pose_name)] = []
for k in joint_names:
joint_angle = pose['joints'][k]
data_out[group][str(pose_name)].append(joint_angle)
print stored_poses_yaml_destination
spartanUtils.saveToYaml(data_out, stored_poses_yaml_destination)
def downsample_by_pose_difference_threshold(images_dir_full_path,
linear_distance_threshold,
rotation_angle_threshold):
"""
Downsamples poses and keeps only those that are sufficiently apart
:param images_dir_full_path:
:type images_dir_full_path:
:param linear_distance_threshold: threshold on the translation, in meters
:type linear_distance_threshold:
:param rotation_angle_threshold: threshold on the angle between the rotations, in degrees
:type rotation_angle_threshold:
:return:
:rtype:
"""
pose_yaml = os.path.join(images_dir_full_path, "pose_data.yaml")
pose_dict = spartanUtils.getDictFromYamlFilename(pose_yaml)
images_dir_temp_path = os.path.join(
os.path.dirname(images_dir_full_path), 'images_temp')
if not os.path.isdir(images_dir_temp_path):
os.makedirs(images_dir_temp_path)
previous_pose_pos = FusionServer.get_numpy_position_from_pose(
pose_dict[0])
previous_pose_quat = FusionServer.get_quaternion_from_pose(
pose_dict[0])
print "Using downsampling by pose difference threshold... "
print "Previously: ", len(pose_dict), " images"
num_kept_images = 0
num_deleted_images = 0
for i in range(0, len(pose_dict)):
single_frame_data = pose_dict[i]
this_pose_pos = FusionServer.get_numpy_position_from_pose(
pose_dict[i])
this_pose_quat = FusionServer.get_quaternion_from_pose(
pose_dict[i])
linear_distance = np.linalg.norm(this_pose_pos - previous_pose_pos)
rotation_distance = spartanUtils.compute_angle_between_quaternions(
this_pose_quat, previous_pose_quat)
if i == 0:
keep_image = True
num_kept_images += 1
elif (linear_distance > linear_distance_threshold) or (
np.rad2deg(rotation_distance) > rotation_angle_threshold):
previous_pose_pos = this_pose_pos
previous_pose_quat = this_pose_quat
num_kept_images += 1
else:
# delete pose from forward kinematics
del pose_dict[i]
continue
# if we have gotten here, then move the images over to the new directory
rgb_filename = os.path.join(
images_dir_full_path, single_frame_data['rgb_image_filename'])
rgb_filename_temp = os.path.join(
images_dir_temp_path, single_frame_data['rgb_image_filename'])
shutil.move(rgb_filename, rgb_filename_temp)
depth_filename = os.path.join(
images_dir_full_path,
single_frame_data['depth_image_filename'])
depth_filename_temp = os.path.join(
images_dir_temp_path,
single_frame_data['depth_image_filename'])
shutil.move(depth_filename, depth_filename_temp)
# # delete pose from posegraph
# del posegraph_list[i-num_deleted_images]
# num_deleted_images += 1
# write downsamples pose_data.yaml (forward kinematics)
spartanUtils.saveToYaml(
pose_dict, os.path.join(images_dir_temp_path, 'pose_data.yaml'))
# remove old images
shutil.move(os.path.join(images_dir_full_path, 'camera_info.yaml'),
os.path.join(images_dir_temp_path, 'camera_info.yaml'))
shutil.rmtree(images_dir_full_path)
print "renaming %s to %s " % (images_dir_temp_path,
images_dir_full_path)
# rename temp images to images
os.rename(images_dir_temp_path, images_dir_full_path)
print "After: ", num_kept_images, " images"
def process_ros_bag(self, bag, output_dir, rgb_only=False):
image_topics = []
for key, topic in self.topics_dict.iteritems():
image_topics.append(topic)
# load all the images
rgb_data = dict()
rgb_data['msgs'] = []
rgb_data['cv_img'] = []
rgb_data['timestamps'] = []
depth_data = dict()
depth_data['msgs'] = []
depth_data['cv_img'] = []
depth_data['timestamps'] = []
depth_data['camera_to_world'] = []
print "image_topics: ", image_topics
# extract TF information
tf_t = rosUtils.setup_tf_transformer_from_ros_bag(bag,
cache_time_secs=3600)
log_rate = 100
counter = 0
for topic, msg, t in bag.read_messages(topics=image_topics):
counter += 1
# skip the first 30 images due to transform errors . . .
# if counter < 30:
# continue
if counter % log_rate == 0:
print "processing image message %d" % (counter)
data = None
if "rgb" in topic:
cv_img = self.cv_bridge.imgmsg_to_cv2(
msg, desired_encoding=self.rgb_encoding)
data = rgb_data
elif "depth" in topic:
cv_img = rosUtils.depth_image_to_cv2_uint16(
msg, bridge=self.cv_bridge)
data = depth_data
try:
# rot ix (x,y,z,w)
(trans,
rot) = tf_t.lookupTransform(self.world_frame,
self.camera_frame,
msg.header.stamp)
except:
print "wasn't able to get transform for image message %d, skipping" % (
counter)
continue
depth_data['camera_to_world'].append((trans, rot))
# save the relevant data
data['msgs'].append(msg)
data['cv_img'].append(cv_img)
data['timestamps'].append(msg.header.stamp.to_nsec())
print "Extracted %d rgb images" % (len(rgb_data['msgs']))
print "Extracted %d depth images" % (len(depth_data['msgs']))
rgb_data['timestamps'] = np.array(rgb_data['timestamps'])
# synchronize the images
synchronized_rgb_imgs = []
for idx, stamp in enumerate(depth_data['timestamps']):
rgb_idx = ImageCapture.lookup_synchronized_image(
stamp, rgb_data['timestamps'])
synchronized_rgb_imgs.append(rgb_data['cv_img'][rgb_idx])
if idx % log_rate == 0:
print "depth image %d matched to rgb image %d" % (idx, rgb_idx)
# save to a file
if not os.path.isdir(output_dir):
os.makedirs(output_dir)
pose_data = dict()
for idx, depth_img in enumerate(depth_data['cv_img']):
rgb_img = synchronized_rgb_imgs[idx]
rgb_filename = "%06i_%s.png" % (idx, "rgb")
rgb_filename_full = os.path.join(output_dir, rgb_filename)
depth_filename = "%06i_%s.png" % (idx, "depth")
depth_filename_full = os.path.join(output_dir, depth_filename)
if idx % log_rate == 0:
print "writing image %d to file %s" % (idx, rgb_filename)
cv2.imwrite(rgb_filename_full, rgb_img)
if not rgb_only:
cv2.imwrite(depth_filename_full, depth_img)
pose_data[idx] = dict()
d = pose_data[idx]
trans, rot = depth_data['camera_to_world'][idx]
quat_wxyz = [rot[3], rot[0], rot[1], rot[2]]
transform_dict = spartanUtils.dictFromPosQuat(trans, quat_wxyz)
d['camera_to_world'] = transform_dict
d['timestamp'] = depth_data['timestamps'][idx]
d['rgb_image_filename'] = rgb_filename
d['depth_image_filename'] = depth_filename
spartanUtils.saveToYaml(pose_data,
os.path.join(output_dir, 'pose_data.yaml'))
# extract the camera info msg
camera_info_msg = None
for topic, msg, t in bag.read_messages(topics=self.camera_info_topic):
camera_info_msg = msg
break
camera_info_dict = rosUtils.camera_info_dict_from_camera_info_msg(
camera_info_msg)
# NOTE: currently the batch_extract_and_fuse_all_scenes.py
# script checks for the existence of this file (camera_info.yaml)
# to determine if the extraction process was completed.
spartanUtils.saveToYaml(camera_info_dict,
os.path.join(output_dir, 'camera_info.yaml'))
def do_main():
rospy.init_node('simple_teleop', anonymous=True)
# setup listener for tf2s (used for ee and controller poses)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
robotSubscriber = JointStateSubscriber("/joint_states")
# wait until robot state found
robotSubscriber = ros_utils.JointStateSubscriber("/joint_states")
print("Waiting for full kuka state...")
while len(robotSubscriber.joint_positions.keys()) < 3:
rospy.sleep(0.1)
print("got full state")
# init gripper
handDriver = SchunkDriver()
# init mouse manager
mouse_manager = TeleopMouseManager()
roll_goal = 0.0
yaw_goal = 0.0
pitch_goal = 0.0
# Start by moving to an above-table pregrasp pose that we know
# EE control will work well from (i.e. far from singularity)
stored_poses_dict = spartan_utils.getDictFromYamlFilename("../station_config/RLG_iiwa_1/stored_poses.yaml")
above_table_pre_grasp = stored_poses_dict["Grasping"]["above_table_pre_grasp"]
robotService = ros_utils.RobotService.makeKukaRobotService()
success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=5)
print("Moved to position")
gripper_goal_pos = 0.0
handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01)
print("sent close goal to gripper")
time.sleep(2)
gripper_goal_pos = 0.1
handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01)
print("sent open goal to gripper")
frame_name = "iiwa_link_ee" # end effector frame name
for i in range(10):
if i == 9:
print "Couldn't find robot pose"
sys.exit(0)
try:
ee_pose_above_table = ros_utils.poseFromROSTransformMsg(
tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform)
ee_tf_above_table = tf_matrix_from_pose(ee_pose_above_table)
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print("Troubling looking up robot pose...")
time.sleep(0.1)
# Then kick off task space streaming
sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming',
robot_msgs.srv.StartStreamingPlan)
init = robot_msgs.srv.StartStreamingPlanRequest()
init.force_guard.append(make_force_guard_msg(20.))
res = sp(init)
print("Started task space streaming")
pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint',
robot_msgs.msg.CartesianGoalPoint, queue_size=1);
def cleanup():
rospy.wait_for_service("plan_runner/stop_plan")
sp = rospy.ServiceProxy('plan_runner/stop_plan',
std_srvs.srv.Trigger)
init = std_srvs.srv.TriggerRequest()
print sp(init)
print("Done cleaning up and stopping streaming plan")
rospy.on_shutdown(cleanup)
br = tf.TransformBroadcaster()
rate = rospy.Rate(100) # max rate at which control should happen
ee_tf_last_commanded = np.zeros((4,4))
def get_initial_pose():
while not rospy.is_shutdown():
# get current tf from ros world to ee
try:
ee_pose_current = ros_utils.poseFromROSTransformMsg(
tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform)
ee_tf_last_commanded = tf_matrix_from_pose(ee_pose_current)
return ee_tf_last_commanded
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print("Troubling looking up robot pose...")
rate.sleep()
continue
print ee_tf_last_commanded
ee_tf_last_commanded = get_initial_pose()
print ee_tf_last_commanded
sys.path.append("../imitation_tools/scripts")
from capture_imitation_data_client import start_bagging_imitation_data_client, stop_bagging_imitation_data_client
start_bagging_imitation_data_client()
pose_save_counter = 0
saved_pose_dict = dict()
saved_pose_counter = 0
try:
# control loop
while not rospy.is_shutdown():
# # get teleop mouse
events = mouse_manager.get_events()
if events["r"]:
print above_table_pre_grasp
print "that was above_table_pre_grasp"
success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=3)
roll_goal = 0.0
yaw_goal = 0.0
pitch_goal = 0.0
ee_tf_last_commanded = get_initial_pose()
continue
pose_save_counter += 1
if events["o"] and pose_save_counter >= 100: # this make it not happen to much
joint_name_list = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7']
joint_position_vector = robotSubscriber.get_position_vector_from_joint_names(joint_name_list)
print joint_position_vector
print "joint positions saved"
new_pose_name = "pose_"+str(saved_pose_counter).zfill(3)
saved_pose_counter += 1
saved_pose_dict[new_pose_name] = joint_position_vector.tolist()
pose_save_counter = 0
if events["escape"]:
stop_bagging_imitation_data_client()
if len(saved_pose_dict) > 0:
spartan_utils.saveToYaml(saved_pose_dict, "saved_poses.yaml")
sys.exit(0)
scale_down = 0.0001
delta_x = events["delta_x"]*scale_down
delta_y = events["delta_y"]*-scale_down
delta_forward = 0.0
forward_scale = 0.001
if events["w"]:
delta_forward -= forward_scale
if events["s"]:
delta_forward += forward_scale
# extract and normalize quat from tf
if events["rotate_left"]:
roll_goal += 0.01
if events["rotate_right"]:
roll_goal -= 0.01
roll_goal = np.clip(roll_goal, a_min = -0.9, a_max = 0.9)
if events["side_button_back"]:
yaw_goal += 0.01
print("side button back")
if events["side_button_forward"]:
yaw_goal -= 0.01
print("side side_button_forward")
yaw_goal = np.clip(yaw_goal, a_min = -1.314, a_max = 1.314)
if events["d"]:
pitch_goal += 0.01
if events["a"]:
pitch_goal -= 0.01
pitch_goal = np.clip(pitch_goal, a_min = -1.314, a_max = 1.314)
R = transformations.euler_matrix(pitch_goal, roll_goal, yaw_goal, 'syxz')
# third is "yaw", when in above table pre-grasp
# second is "roll", ''
# first must be "pitch"
above_table_quat_ee = transformations.quaternion_from_matrix(R.dot(ee_tf_above_table))
above_table_quat_ee = np.array(above_table_quat_ee) / np.linalg.norm(above_table_quat_ee)
# calculate controller position delta and add to start position to get target ee position
target_translation = np.asarray([delta_forward, delta_x, delta_y])
empty_matrx = np.zeros_like(ee_tf_last_commanded)
empty_matrx[:3, 3] = target_translation
ee_tf_last_commanded += empty_matrx
target_trans_ee = ee_tf_last_commanded[:3, 3]
# publish target pose as cartesian goal point
new_msg = robot_msgs.msg.CartesianGoalPoint()
new_msg.xyz_point.header.stamp = rospy.Time.now()
new_msg.xyz_point.header.frame_id = "world"
new_msg.xyz_point.point.x = target_trans_ee[0]
new_msg.xyz_point.point.y = target_trans_ee[1]
new_msg.xyz_point.point.z = target_trans_ee[2]
new_msg.xyz_d_point.x = 0.0
new_msg.xyz_d_point.y = 0.0
new_msg.xyz_d_point.z = 0.0
new_msg.quaternion.w = above_table_quat_ee[0]
new_msg.quaternion.x = above_table_quat_ee[1]
new_msg.quaternion.y = above_table_quat_ee[2]
new_msg.quaternion.z = above_table_quat_ee[3]
new_msg.roll = roll_goal
new_msg.pitch = pitch_goal
new_msg.yaw = yaw_goal
new_msg.gain = make_cartesian_gains_msg(5., 10.)
new_msg.ee_frame_id = frame_name
pub.publish(new_msg)
#gripper
if events["mouse_wheel_up"]:
gripper_goal_pos += 0.006
if events["mouse_wheel_down"]:
gripper_goal_pos -= 0.006
if gripper_goal_pos < 0:
gripper_goal_pos = 0.0
if gripper_goal_pos > 0.1:
gripper_goal_pos = 0.1
handDriver.sendGripperCommand(gripper_goal_pos, speed=0.2, stream=True)
rate.sleep()
except Exception as e:
print "Suffered exception ", e
def saveData(self):
filename = os.path.join(self.folderName, 'data.yaml')
spartanUtils.saveToYaml(self.data, filename)
def runROSCalibrationFixedMount(self, headerData, topic_name, poses_file):
headerData['target']['transform_to_hand_frame'] = dict()
headerData['target']['transform_to_hand_frame']['translation'] = dict()
headerData['target']['transform_to_hand_frame']['translation'][
'x'] = 0.0
headerData['target']['transform_to_hand_frame']['translation'][
'y'] = 0.0
headerData['target']['transform_to_hand_frame']['translation'][
'z'] = 0.0931
headerData['target']['transform_to_hand_frame']['quaternion'] = dict()
headerData['target']['transform_to_hand_frame']['quaternion'][
'x'] = 0.49894425
headerData['target']['transform_to_hand_frame']['quaternion'][
'y'] = 0.50080633
headerData['target']['transform_to_hand_frame']['quaternion'][
'z'] = 0.51116578
headerData['target']['transform_to_hand_frame']['quaternion'][
'w'] = -0.48883249
self.calibrationData = dict()
calibrationRunData = self.calibrationData
calibrationRunData['header'] = headerData
self.passiveSubscriber = spartanROSUtils.SimpleSubscriber(
topic_name, sensor_msgs.msg.Image)
self.passiveSubscriber.start()
unique_name = time.strftime(
"%Y%m%d-%H%M%S") + "_" + self.calibrationType
self.calibrationFolderName = os.path.join(
spartanUtils.getSpartanSourceDir(), 'calibration_data',
unique_name)
os.system("mkdir -p " + self.calibrationFolderName)
os.chdir(self.calibrationFolderName)
rospy.loginfo("starting calibration run")
calibrationData = []
poses = spartanUtils.getDictFromYamlFilename(poses_file)
num_poses = len(poses)
for index, pose_name in enumerate(sorted(poses.keys())):
# move robot to that joint position
info_msg = "\n moving to pose " + str(index) + " of " + str(
num_poses) + " named " + pose_name
rospy.loginfo(info_msg)
self.robotService.moveToJointPosition(poses[pose_name])
rospy.loginfo("capturing images and robot data")
data = self.captureCurrentRobotAndImageData(
captureRGB=self.captureRGB,
captureIR=self.captureIR,
prefix=str(index))
calibrationData.append(data)
rospy.loginfo("finished calibration routine, saving data to file")
calibrationRunData['data_list'] = calibrationData
spartanUtils.saveToYaml(
calibrationRunData,
os.path.join(self.calibrationFolderName, 'robot_data.yaml'))
if self.passiveSubscriber:
self.passiveSubscriber.stop()
self.moveHome()
return calibrationRunData
def save_transform_to_file(transform):
filename = os.path.join(spartan_utils.get_sandbox_dir(), "transform.yaml")
d = poseFromTransform(transform)
spartan_utils.saveToYaml(d, filename)
print("saved transform to %s" % (filename))
