00001 import rosbag
00002 import tf
00003 import copy
00004 import math
00005 import matplotlib
00006 matplotlib.use('Agg')
00007 from matplotlib import pyplot
00008 import numpy
00009 from scipy import signal
00010 from geometry_msgs.msg import PoseStamped, Quaternion, Point
00011
00012
00013 __author__ = "Fadri Furrer, Michael Burri, Markus Achtelik"
00014 __copyright__ = ("Copyright 2015, Fadri Furrer & Michael Burri & "
00015 "Markus Achtelik, ASL, ETH Zurich, Switzerland")
00016 __credits__ = ["Fadri Furrer", "Michael Burri", "Markus Achtelik"]
00017 __license__ = "ASL 2.0"
00018 __version__ = "0.1"
00019 __maintainer__ = "Fadri Furrer"
00020 __email__ = "fadri.furrer@mavt.ethz.ch"
00021 __status__ = "Development"
00022
00023
00024 class BaseWithTime(object):
00025
00026 """Base Class for all objects with time and bag_time."""
00027
00028 def __init__(self):
00029 self.time = numpy.array([])
00030 self.bag_time = numpy.array([])
00031
00032 def append_times(self, msg_time, bag_time):
00033 """Append the msg_time and the bag_time."""
00034 self.time = numpy.append(self.time, msg_time)
00035 self.bag_time = numpy.append(self.bag_time, bag_time)
00036
00037 def slice(self, start_time, end_time):
00038 """Copy the object and slice it between start_time and end_time."""
00039 start_index = self.get_next_index(start_time)
00040 end_index = self.get_next_index(end_time)
00041 copied_obj = copy.deepcopy(self)
00042 for key in self.__dict__:
00043 copied_obj.__setattr__(
00044 key, copied_obj.__getattribute__(key)[start_index:end_index])
00045 return copied_obj
00046
00047 def get_next_index(self, time):
00048 """Get the index of the next value after time."""
00049 try:
00050 index = next(idx for idx, val in enumerate(self.time) if val > time)
00051 except StopIteration:
00052 index = len(self.time)
00053 except Exception, e:
00054 raise e
00055 return index
00056
00057
00058 class ArrayWithTime(BaseWithTime):
00059
00060 """This class stores arrays or lists and its corresponding times."""
00061
00062 def __init__(self):
00063 self.data = numpy.array([])
00064 BaseWithTime.__init__(self)
00065
00066 def append_array(self, array):
00067 """Append an array or list to the data array."""
00068 try:
00069 self.data = numpy.append(self.data, [numpy.array(array)], axis=0)
00070 except ValueError:
00071
00072 if self.data.size == 0:
00073 self.data = numpy.array([numpy.array(array)])
00074 else:
00075 raise ValueError
00076
00077
00078 class QuatWithTime(BaseWithTime):
00079
00080 """This class stores quaternions and its corresponding times."""
00081
00082 def __init__(self):
00083 self.w = numpy.array([])
00084 self.x = numpy.array([])
00085 self.y = numpy.array([])
00086 self.z = numpy.array([])
00087 BaseWithTime.__init__(self)
00088
00089 def append_quaternion(self, quaternion_msg):
00090 """Append the w, x, y, z components from a quaternion to its arrays."""
00091 self.w = numpy.append(self.w, quaternion_msg.w)
00092 self.x = numpy.append(self.x, quaternion_msg.x)
00093 self.y = numpy.append(self.y, quaternion_msg.y)
00094 self.z = numpy.append(self.z, quaternion_msg.z)
00095
00096
00097 class XYZWithTime(BaseWithTime):
00098
00099 """This class stores x,y,z and its corresponding times."""
00100
00101 def __init__(self):
00102 self.x = numpy.array([])
00103 self.y = numpy.array([])
00104 self.z = numpy.array([])
00105 BaseWithTime.__init__(self)
00106
00107 def append_point(self, point_msg):
00108 """Append the x, y, z components from a point to its arrays."""
00109 self.x = numpy.append(self.x, point_msg.x)
00110 self.y = numpy.append(self.y, point_msg.y)
00111 self.z = numpy.append(self.z, point_msg.z)
00112
00113 def resample(self, sampling_times):
00114
00115 [self.x, self.time] = signal.resample(
00116 self.x,
00117 len(sampling_times),
00118 sampling_times)
00119 [self.y, self.time] = signal.resample(
00120 self.y,
00121 len(sampling_times),
00122 sampling_times)
00123 [self.z, self.time] = signal.resample(
00124 self.z,
00125 len(sampling_times),
00126 sampling_times)
00127
00128
00129 class WrenchWithTime(BaseWithTime):
00130
00131 """
00132 This class stores x,y,z-forces and torques with its corresponding times.
00133 """
00134
00135 def __init__(self):
00136 self.force_x = numpy.array([])
00137 self.force_y = numpy.array([])
00138 self.force_z = numpy.array([])
00139 self.torque_x = numpy.array([])
00140 self.torque_y = numpy.array([])
00141 self.torque_z = numpy.array([])
00142 BaseWithTime.__init__(self)
00143
00144 def append_wrench(self, wrench_msg):
00145 """
00146 Append the x, y, z components for force and torque to their arrays.
00147 """
00148 self.force_x = numpy.append(self.force_x, wrench_msg.wrench.force.x)
00149 self.force_y = numpy.append(self.force_y, wrench_msg.wrench.force.y)
00150 self.force_z = numpy.append(self.force_z, wrench_msg.wrench.force.z)
00151 self.torque_x = numpy.append(self.torque_x, wrench_msg.wrench.torque.x)
00152 self.torque_y = numpy.append(self.torque_y, wrench_msg.wrench.torque.y)
00153 self.torque_z = numpy.append(self.torque_z, wrench_msg.wrench.torque.z)
00154
00155
00156 class WaypointWithTime(XYZWithTime):
00157
00158 """This class stores waypoints or and its corresponding times."""
00159
00160 def __init__(self):
00161 self.yaw = numpy.array([])
00162 XYZWithTime.__init__(self)
00163 self.empty = True
00164
00165 def append_waypoint(self, waypoint_msg, msg_time, bag_time):
00166 """Append data from a waypoint to its arrays."""
00167 position = waypoint_msg.points[0].transforms[0].translation
00168 rotation = waypoint_msg.points[0].transforms[0].rotation
00169 quaternion = (rotation.x, rotation.y, rotation.z, rotation.w)
00170 euler = tf.transformations.euler_from_quaternion(quaternion)
00171 point_msg = Point(position.x, position.y, position.z)
00172 yaw = euler[2]
00173
00174 try:
00175 different_waypoint = (self.yaw[-1] != yaw or
00176 self.x[-1] != point_msg.x or
00177 self.y[-1] != point_msg.y or
00178 self.z[-1] != point_msg.z)
00179 except IndexError:
00180 different_waypoint = True
00181
00182 if (different_waypoint):
00183 self.append_point(point_msg)
00184 self.yaw = numpy.append(self.yaw, yaw)
00185 self.append_times(msg_time, bag_time)
00186
00187
00188 class RPYWithTime(BaseWithTime):
00189
00190 """This class stores roll, pitch, yaw and its corresponding times."""
00191
00192 def __init__(self):
00193 self.roll = numpy.array([])
00194 self.pitch = numpy.array([])
00195 self.yaw = numpy.array([])
00196 BaseWithTime.__init__(self)
00197
00198 def append_quaternion(self, quaternion_msg):
00199 """Append a roll, pitch, and yaw to each array from a quaternion."""
00200 quaternion = (
00201 quaternion_msg.x,
00202 quaternion_msg.y,
00203 quaternion_msg.z,
00204 quaternion_msg.w)
00205 euler = tf.transformations.euler_from_quaternion(quaternion)
00206 self.roll = numpy.append(self.roll, euler[0] * 180 / math.pi)
00207 self.pitch = numpy.append(self.pitch, euler[1] * 180 / math.pi)
00208 self.yaw = numpy.append(self.yaw, euler[2] * 180 / math.pi)
00209
00210
00211 class AnalyzeBag(object):
00212
00213 """This class can be used to plot data and compare data."""
00214
00215 def __init__(self, bag_path_name, save_plots, prefix=None):
00216 self.bag = rosbag.Bag(bag_path_name)
00217 self.topics = []
00218 self.pose_topics = []
00219 self.twist_topics = []
00220 self.imu_topics = []
00221 self.motor_velocity_topics = []
00222 self.waypoint_topics = []
00223 self.wrench_topics = []
00224 self.pos = []
00225 self.quat = []
00226 self.rpy = []
00227 self.pqr = []
00228 self.motor_vel = []
00229 self.acc = []
00230 self.ang_vel = []
00231 self.waypoint = []
00232 self.wrench = []
00233 self.save_plots = save_plots
00234 self.bag_time_start = None
00235 self.bag_time_end = None
00236 self.prefix = prefix
00237
00238 def add_pose_topic(self, topic):
00239 """Add a pose topic that should be analyzed."""
00240 self.pos.append(XYZWithTime())
00241 self.quat.append(QuatWithTime())
00242 self.rpy.append(RPYWithTime())
00243 self.pose_topics.append(topic)
00244 self.topics.append(topic)
00245
00246 def add_twist_topic(self, topic):
00247 """Add a twist topic that should be analyzed."""
00248 self.pqr.append(XYZWithTime())
00249 self.twist_topics.append(topic)
00250 self.topics.append(topic)
00251
00252 def add_imu_topic(self, topic):
00253 """Add an imu topic that should be analyzed."""
00254 self.acc.append(XYZWithTime())
00255 self.ang_vel.append(XYZWithTime())
00256 self.imu_topics.append(topic)
00257 self.topics.append(topic)
00258
00259 def add_motor_velocity_topic(self, topic):
00260 """Add a motor velocity topic."""
00261 self.motor_vel.append(ArrayWithTime())
00262 self.motor_velocity_topics.append(topic)
00263 self.topics.append(topic)
00264
00265 def add_waypoint_topic(self, topic):
00266 """Add a waypoint topic that should be analyzed."""
00267 self.waypoint.append(WaypointWithTime())
00268 self.waypoint_topics.append(topic)
00269 self.topics.append(topic)
00270
00271 def add_wrench_topic(self, topic):
00272 """Add a topic of the wrench."""
00273 self.wrench.append(WrenchWithTime())
00274 self.wrench_topics.append(topic)
00275 self.topics.append(topic)
00276
00277 def extract_messages(self):
00278 """Run through the bag file and assign the msgs to its attributes."""
00279 for topic, msg, bag_time in self.bag.read_messages(topics=self.topics):
00280 if self.bag_time_start is None:
00281 self.bag_time_start = bag_time
00282 self.extract_pose_topics(topic, msg, bag_time)
00283 self.extract_imu_topics(topic, msg, bag_time)
00284 self.extract_twist_topics(topic, msg, bag_time)
00285 self.extract_motor_velocity_topics(topic, msg, bag_time)
00286 self.extract_waypoint_topics(topic, msg, bag_time)
00287 self.extract_wrench_topics(topic, msg, bag_time)
00288 self.bag_time_end = bag_time
00289
00290 def extract_pose_topics(self, topic, msg, bag_time):
00291 """Append the pose topic msg content to the rpy and pos attributes."""
00292 msg_time = msg.header.stamp.to_sec()
00293
00294 pose_msg = PoseStamped()
00295 pose_msg.pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
00296 for index, pose_topic in enumerate(self.pose_topics):
00297 if topic != pose_topic:
00298 continue
00299 elif "PoseStamped" in str(type(msg)):
00300 self.pos[index].append_point(msg.pose.position)
00301 self.pos[index].append_times(msg_time, bag_time)
00302 self.quat[index].append_quaternion(msg.pose.orientation)
00303 self.quat[index].append_times(msg_time, bag_time)
00304 self.rpy[index].append_quaternion(msg.pose.orientation)
00305 self.rpy[index].append_times(msg_time, bag_time)
00306 elif "TransformStamped" in str(type(msg)):
00307 self.pos[index].append_point(msg.transform.translation)
00308 self.pos[index].append_times(msg_time, bag_time)
00309 self.quat[index].append_quaternion(msg.transform.rotation)
00310 self.quat[index].append_times(msg_time, bag_time)
00311 self.rpy[index].append_quaternion(msg.transform.rotation)
00312 self.rpy[index].append_times(msg_time, bag_time)
00313 elif "PointStamped" in str(type(msg)):
00314 self.pos[index].append_point(msg.point)
00315 self.pos[index].append_times(msg_time, bag_time)
00316 self.quat[index].append_quaternion(pose_msg.pose.orientation)
00317 self.quat[index].append_times(msg_time, bag_time)
00318 self.rpy[index].append_quaternion(pose_msg.pose.orientation)
00319 self.rpy[index].append_times(msg_time, bag_time)
00320 else:
00321 print("Got unknown type: %s" % type(msg))
00322
00323 def extract_imu_topics(self, topic, msg, bag_time):
00324 """Append the imu topic msg content to the acc attributes."""
00325 msg_time = msg.header.stamp.to_sec()
00326 for index, imu_topic in enumerate(self.imu_topics):
00327 if topic == imu_topic:
00328 self.acc[index].x.append(msg.linear_acceleration.x)
00329 self.acc[index].y.append(msg.linear_acceleration.y)
00330 self.acc[index].z.append(msg.linear_acceleration.z)
00331 self.acc[index].append_times(msg_time, bag_time)
00332
00333 def extract_twist_topics(self, topic, msg, bag_time):
00334 """Append the twist topic msg content to the pqr attributes."""
00335 msg_time = msg.header.stamp.to_sec()
00336 for index, twist_topic in enumerate(self.twist_topics):
00337 if topic == twist_topic:
00338 self.pqr[index].append_point(msg.twist.angular)
00339 self.pqr[index].append_times(msg_time, bag_time)
00340
00341 def extract_motor_velocity_topics(self, topic, msg, bag_time):
00342 """
00343 Append the motor velocity topic msg content to the motor_vel
00344 attributes.
00345 """
00346 msg_time = msg.header.stamp.to_sec()
00347 for index, motor_velocity_topic in enumerate(self.motor_velocity_topics):
00348 if topic == motor_velocity_topic:
00349 self.motor_vel[index].append_array(msg.motor_speed)
00350 self.motor_vel[index].append_times(msg_time, bag_time)
00351
00352 def extract_waypoint_topics(self, topic, msg, bag_time):
00353 """Append the waypoint topic msg to the waypoint attributes."""
00354 msg_time = msg.header.stamp.to_sec()
00355 for index, waypoint_topic in enumerate(self.waypoint_topics):
00356 if topic == waypoint_topic:
00357 self.waypoint[index].append_waypoint(msg, msg_time, bag_time)
00358
00359 def extract_wrench_topics(self, topic, msg, bag_time):
00360 """Append the wrench topic msg to the wrench attributes."""
00361 msg_time = msg.header.stamp.to_sec()
00362 for index, wrench_topic in enumerate(self.wrench_topics):
00363 if topic == wrench_topic:
00364 self.wrench[index].append_wrench(msg)
00365 self.wrench[index].append_times(msg_time, bag_time)
00366
00367 def plot_positions(self, start_time=None, end_time=None,
00368 settling_time=None, x_range=None, y_range=None,
00369 plot_suffix=None):
00370 """Plot all position lists."""
00371 fig = pyplot.figure()
00372 fig.suptitle("Position")
00373 a_x = fig.add_subplot(111)
00374 for index, pos in enumerate(self.pos):
00375
00376 a_x.plot(pos.time, pos.x, 'b', label='x')
00377 a_x.plot(pos.time, pos.y, 'r', label='y')
00378 a_x.plot(pos.time, pos.z, 'g', label='z')
00379
00380 if not y_range:
00381 y_max = max([max(pos.x), max(pos.y), max(pos.z)])
00382 y_min = min([min(pos.x), min(pos.y), min(pos.z)])
00383 else:
00384 y_max = y_range[1]
00385 y_min = y_range[0]
00386 y_center = (y_max + y_min)/2.0
00387
00388 if start_time:
00389 a_x.axvline(x=start_time, color='c')
00390 pyplot.text(start_time, y_center, 'start evaluation', rotation=90,
00391 color='c')
00392 if end_time:
00393 a_x.axvline(x=end_time, color='m')
00394 pyplot.text(end_time, y_max, 'end evaluation', rotation=90,
00395 color='m')
00396 if settling_time:
00397 a_x.axvline(x=settling_time, color='k')
00398 pyplot.text(settling_time, y_max, 'settled, start RMS eval',
00399 rotation=90, color='k')
00400
00401 pyplot.xlabel('time [s]')
00402 pyplot.ylabel('position [m]')
00403
00404 box = a_x.get_position()
00405 a_x.set_position([box.x0, box.y0 + box.height * 0.2,
00406 box.width, box.height * 0.8])
00407
00408
00409 a_x.legend(loc='upper center', bbox_to_anchor=(0.5, -0.15),
00410 fancybox=True, shadow=True, ncol=5)
00411 pyplot.xlim(x_range)
00412 pyplot.ylim(y_range)
00413 pyplot.grid(b=True, which='both')
00414
00415 if self.save_plots:
00416 file_name = self.prefix + '_pos' if self.prefix else 'pos'
00417 if plot_suffix:
00418 file_name += '_' + str(plot_suffix)
00419 file_name += '.png'
00420 pyplot.savefig(file_name)
00421
00422 def plot_position_error(self, set_point, settling_radius=None,
00423 start_time=None, end_time=None, settling_time=None,
00424 x_range=None, y_range=None, plot_suffix=None):
00425 """Plot all position errors lists."""
00426 fig = pyplot.figure()
00427 fig.suptitle("Position error")
00428 a_x = fig.add_subplot(111)
00429 pos_errors = numpy.array([])
00430
00431 for index, pos in enumerate(self.pos):
00432 x_error = (pos.x - set_point.x[0])
00433 y_error = (pos.y - set_point.y[0])
00434 z_error = (pos.z - set_point.z[0])
00435
00436 pos_errors = numpy.append(pos_errors,
00437 (x_error ** 2 + y_error ** 2
00438 + z_error ** 2) ** 0.5)
00439 a_x.plot(pos.time, pos_errors, 'b', label='pos_error')
00440
00441 if not y_range:
00442 y_max = max(pos_errors)
00443 y_min = 0
00444 else:
00445 y_max = y_range[1]
00446 y_min = y_range[0]
00447 y_center = (y_max + y_min)/2.0
00448
00449 if settling_radius:
00450 a_x.axhline(y=settling_radius, color='r', xmin=0, xmax=1)
00451 pyplot.text(start_time, settling_radius, 'settling radius',
00452 color='r')
00453 if start_time:
00454 a_x.axvline(x=start_time, color='c')
00455 pyplot.text(start_time, y_center, 'start evaluation', rotation=90,
00456 color='c')
00457 if end_time:
00458 a_x.axvline(x=end_time, color='m')
00459 pyplot.text(end_time, y_max, 'end evaluation', rotation=90,
00460 color='m')
00461 if settling_time:
00462 a_x.axvline(x=settling_time, color='k')
00463 pyplot.text(settling_time, y_max, 'settled, start RMS eval',
00464 rotation=90, color='k')
00465
00466 pyplot.xlabel('time [s]')
00467 pyplot.ylabel('position error [m]')
00468
00469 box = a_x.get_position()
00470 a_x.set_position([box.x0, box.y0 + box.height * 0.2,
00471 box.width, box.height * 0.8])
00472
00473
00474 a_x.legend(loc='upper center', bbox_to_anchor=(0.5, -0.15),
00475 fancybox=True, shadow=True, ncol=5)
00476 pyplot.xlim(x_range)
00477 pyplot.ylim(y_range)
00478 pyplot.grid(b=True, which='both')
00479
00480 if self.save_plots:
00481 file_name = self.prefix + '_pos_error' if self.prefix else 'pos_error'
00482 if plot_suffix:
00483 file_name += '_' + str(plot_suffix)
00484 file_name += '.png'
00485 pyplot.savefig(file_name)
00486
00487
00488
00489 def plot_angular_velocities(self, start_time=None, end_time=None,
00490 settling_time=None, x_range=None, y_range=None,
00491 plot_suffix=None):
00492 """Plot all angular_velocity lists."""
00493 fig = pyplot.figure()
00494 fig.suptitle("Angular Velocity")
00495 a_x = fig.add_subplot(111)
00496 for index, pqr in enumerate(self.pqr):
00497 a_x.plot(pqr.time, pqr.x, 'b', label='x')
00498 a_x.plot(pqr.time, pqr.y, 'r', label='y')
00499 a_x.plot(pqr.time, pqr.z, 'g', label='z')
00500
00501 if not y_range:
00502 y_max = max([max(pqr.x), max(pqr.y), max(pqr.z)])
00503 y_min = min([min(pqr.x), min(pqr.y), min(pqr.z)])
00504 else:
00505 y_max = y_range[1]
00506 y_min = y_range[0]
00507 y_center = (y_max + y_min)/2.0
00508
00509 if start_time:
00510 a_x.axvline(x=start_time, color='c')
00511 pyplot.text(start_time, y_center, 'start evaluation', rotation=90,
00512 color='c')
00513 if end_time:
00514 a_x.axvline(x=end_time, color='m')
00515 pyplot.text(end_time, y_max, 'end evaluation', rotation=90,
00516 color='m')
00517 if settling_time:
00518 a_x.axvline(x=settling_time, color='k')
00519 pyplot.text(settling_time, y_max, 'settled, start RMS eval',
00520 rotation=90, color='k')
00521
00522 pyplot.xlabel('time [s]')
00523 pyplot.ylabel('angular velocity [rad/s]')
00524
00525 box = a_x.get_position()
00526 a_x.set_position([box.x0, box.y0 + box.height * 0.2,
00527 box.width, box.height * 0.8])
00528
00529
00530 a_x.legend(loc='upper center', bbox_to_anchor=(0.5, -0.15),
00531 fancybox=True, shadow=True, ncol=5)
00532 pyplot.xlim(x_range)
00533 pyplot.ylim(y_range)
00534 pyplot.grid(b=True, which='both')
00535
00536 if self.save_plots:
00537 file_name = self.prefix + '_angular_velocity' if self.prefix else 'angular_velocity'
00538 if plot_suffix:
00539 file_name += '_' + str(plot_suffix)
00540 file_name += '.png'
00541 pyplot.savefig(file_name)
00542
00543
00544
00545 def plot_3d_trajectories(self):
00546 fig = pyplot.figure()
00547 a_x = fig.add_subplot(111, projection='3d')
00548 for index, pos in enumerate(self.pos):
00549 a_x.plot(pos.x, pos.y, pos.z, label=self.pose_topics[index])
00550
00551
00552 def plot_rpys(self, start_time=None, end_time=None, settling_time=None,
00553 plot_suffix=None):
00554 """Plot rpy lists."""
00555 fig = pyplot.figure()
00556 fig.suptitle("RPY")
00557 a_x = fig.add_subplot(111)
00558 for index, rpy in enumerate(self.rpy):
00559 a_x.plot(rpy.time, rpy.roll, 'b',
00560 label='roll' + self.pose_topics[index])
00561 a_x.plot(rpy.time, rpy.pitch, 'r',
00562 label='pitch' + self.pose_topics[index])
00563 a_x.plot(rpy.time, rpy.yaw, 'g',
00564 label='yaw' + self.pose_topics[index])
00565
00566 if start_time:
00567 a_x.axvline(x=start_time, color='c')
00568 if end_time:
00569 a_x.axvline(x=end_time, color='m')
00570 if settling_time:
00571 a_x.axvline(x=settling_time, color='k')
00572
00573 pyplot.xlabel('time [s]')
00574 pyplot.ylabel('angle [deg]')
00575 pyplot.legend()
00576
00577 if self.save_plots:
00578 file_name = self.prefix + '_rpy' if self.prefix else 'rpy'
00579 if plot_suffix:
00580 file_name += '_' + str(plot_suffix)
00581 file_name += '.png'
00582 pyplot.savefig(file_name)
00583
00584
00585
00586 def plot_accelerations(self, plot_suffix=None):
00587 """Plot all acceleration lists."""
00588 fig = pyplot.figure()
00589 fig.suptitle("Accelerations")
00590 a_x = fig.add_subplot(111)
00591 for index, acc in enumerate(self.acc):
00592 a_x.plot(acc.time, acc.x, 'b', label='x' + self.imu_topics[index])
00593 a_x.plot(acc.time, acc.y, 'r', label='y' + self.imu_topics[index])
00594 a_x.plot(acc.time, acc.z, 'g', label='z' + self.imu_topics[index])
00595 pyplot.xlabel('time [s]')
00596 pyplot.ylabel('acceleration [m/s^2]')
00597 pyplot.legend()
00598 if self.save_plots:
00599 file_name = self.prefix + '_acc' if self.prefix else 'acc'
00600 if plot_suffix:
00601 file_name += '_' + str(plot_suffix)
00602 file_name += '.png'
00603 pyplot.savefig(file_name)
00604
00605
00606 def compare_positions(self, pose_indeces):
00607 """
00608 Compare the position lists of pose_topics.
00609
00610 Args:
00611 pose_indeces (list): Indeces of pose_topics for comparison.
00612 """
00613 if len(pose_indeces) < 2:
00614 print("At least two pose_indeces need to be provided to make a "
00615 "comparison")
00616 for index, pose_index in enumerate(pose_indeces):
00617 for pose_index_cmp in pose_indeces[index+1:]:
00618 compare_two_xyz(self.pos[pose_index],
00619 self.pos[pose_index_cmp])
00620
00621 def get_collisions(self, start_time=None, end_time=None):
00622 """Get the collision times."""
00623 collision_times = []
00624
00625 for collision in self.wrench:
00626 for t in collision.time:
00627 if t >= (start_time or 0) and t <= (end_time or
00628 max(collision.time)):
00629 collision_times.append(t)
00630 return collision_times
00631
00632
00633 def compare_two_xyz(xyz_one, xyz_two):
00634
00635 pass
00636
00637
00638 def xyz_rms_error(set_point, input_series):
00639 """
00640 Calculate the RMS error between a set_point and the points in input_series.
00641
00642 Args:
00643 set_point: XYZWithTime of the reference point
00644 input_series (list): XYZWithTime of the points that should be evaluated
00645
00646 Returns:
00647 rms: Float of the RMS Error of the XYZWithTime input_series
00648 """
00649 sum_of_squares = 0.0
00650 elems = len(input_series.x)
00651 for index in range(elems):
00652 x_error = (input_series.x[index] - set_point.x)
00653 y_error = (input_series.y[index] - set_point.y)
00654 z_error = (input_series.z[index] - set_point.z)
00655 sum_of_squares += (1.0 / elems *
00656 (x_error ** 2 + y_error ** 2 + z_error ** 2))
00657 rms = sum_of_squares ** 0.5
00658 return rms
00659
00660
00661 def settling_time(set_point, input_series, bounding_radius, min_time):
00662 """
00663 Calculate the settling time of input_series to be bounded for min_time.
00664
00665 Args:
00666 set_point: XYZWithTime of the reference point
00667 input_series (list): XYZWithTime of the points that should be evaluated
00668 bounding_radius: Float of the radius of the bounding sphere
00669 min_time: Float of the time, the input_series has to stay within bounds
00670
00671 Returns:
00672 settling_time: Float of the time for input_series to be settled
00673 """
00674 bounded_time = False
00675 elems = len(input_series.x)
00676 for index in range(elems):
00677 current_time = input_series.time[index]
00678 x_error = (input_series.x[index] - set_point.x)
00679 y_error = (input_series.y[index] - set_point.y)
00680 z_error = (input_series.z[index] - set_point.z)
00681 error_radius = (x_error ** 2 + y_error ** 2 + z_error ** 2) ** 0.5
00682 if error_radius <= bounding_radius and not bounded_time:
00683 bounded_time = current_time
00684 elif (error_radius <= bounding_radius and bounded_time and
00685 current_time - bounded_time >= min_time):
00686 return bounded_time - input_series.time[0]
00687 elif (not bounded_time or error_radius <= bounding_radius and
00688 bounded_time):
00689 continue
00690 else:
00691 bounded_time = False
00692 return None
00693
00694
00695 def work(input_series, work_constant):
00696 """
00697 Calculate the work of input_series.
00698
00699 Args:
00700 input_series (list): ArrayWithTime of the motor velocities
00701 work_constant: Float of the (rotor_constant * moment_constant) constant
00702
00703 Returns:
00704 total_work: Float of the work done in input_series
00705 """
00706 total_work = 0
00707
00708 elems = len(input_series.time)
00709 for index in range(elems - 1):
00710 current_time = input_series.time[index]
00711 next_time = input_series.time[index + 1]
00712 delta_time = next_time - current_time
00713 for value in input_series.data[index]:
00714 total_work += abs(value) ** 3 * work_constant * delta_time
00715 return total_work
00716
00717
00718 def get_errors_pos_only(xyz_one, quat_one, xyz_two, restart_distance,
00719 alignment_length, t_vs=numpy.eye(4), time_offset=0):
00720
00721 xyz_two_time_shifted = copy.deepcopy(xyz_two)
00722 xyz_two_time_shifted.time -= time_offset
00723 xyz_two_time_shifted.resample(xyz_one.time)
00724 xyz_one_time_shifted = xyz_one
00725 xyz_one_time_shifted.time -= time_offset
00726 xyz_one_time_shifted.resample(xyz_two.time)
00727 xyz_two = xyz_two_time_shifted
00728
00729
00730 def create_topic_list(topics_string):
00731 if topics_string and ',' in topics_string:
00732 return topics_string.split(',')
00733 elif topics_string:
00734 return [topics_string]
00735 else:
00736 return []
00737
00738
00739 def create_set_point(x, y, z):
00740 """Create a XYZWithTime set point."""
00741 point_msg = Point(x, y, z)
00742 set_point = XYZWithTime()
00743 set_point.append_point(point_msg)
00744 return set_point