| # Copyright (c) 2013 The Chromium OS Authors. All rights reserved. |
| # Use of this source code is governed by a BSD-style license that can be |
| # found in the LICENSE file. |
| |
| import os |
| import pickle |
| |
| |
| class Position: |
| """ Representation of a robot position |
| |
| The robot position is represented in two ways: |
| Cartesian: x, y, z, yaw, pitch, roll |
| Joint: ax_1, ax_2, ax_3, ax_4, ax_5 |
| |
| This class stores all the variables in one cohesive object |
| to be used for any method that works on robot positions |
| """ |
| |
| def __init__(self, x, y, z, yaw, pitch, roll, ax_1, ax_2, ax_3, ax_4, ax_5): |
| self.x = x |
| self.y = y |
| self.z = z |
| self.yaw = yaw |
| self.pitch = pitch |
| self.roll = roll |
| self.ax_1 = ax_1 |
| self.ax_2 = ax_2 |
| self.ax_3 = ax_3 |
| self.ax_4 = ax_4 |
| self.ax_5 = ax_5 |
| |
| @staticmethod |
| def FromTouchbotResponse(touchbot_response): |
| """ Parse a response from the touchbot and build a Position object """ |
| values = [float(v) for v in touchbot_response.split()] |
| if len(values) != 11: |
| return None |
| x, y, z = values[0:3] |
| yaw, pitch, roll = values[3:6] |
| ax_1, ax_2, ax_3, ax_4, ax_5 = values[6:] |
| return Position(x, y, z, yaw, pitch, roll, ax_1, ax_2, ax_3, ax_4, ax_5) |
| |
| @staticmethod |
| def FromPickledFile(filename): |
| this_dir = os.path.dirname(os.path.abspath(__file__)) |
| filename = os.path.join(this_dir, filename) |
| try: |
| with open(filename, 'rb') as fo: |
| return pickle.load(fo) |
| except: |
| return None |
| |
| def __str__(self): |
| """ Print the position in the format the robot expects """ |
| return '%f %f %f %f %f %f %f %f %f %f %f' % (self.x, self.y, self.z, |
| self.yaw, self.pitch, self.roll, self.ax_1, |
| self.ax_2, self.ax_3, self.ax_4, self.ax_5) |
| |
| def CartesianDistanceFrom(self, other): |
| return ((self.x - other.x) ** 2 + |
| (self.y - other.y) ** 2 + |
| (self.z - other.z) ** 2) ** 0.5 |
