Source code for pyniryo2.objects

#!/usr/bin/env python
# coding=utf-8

from time import time
import numpy as np


[docs]class PoseObject: """ Pose object which stores x, y, z, roll, pitch & yaw parameters :ivar x: X (meter) :vartype x: float :ivar y: Y (meter) :vartype y: float :ivar z: Z (meter) :vartype z: float :ivar roll: Roll (radian) :vartype roll: float :ivar pitch: Pitch (radian) :vartype pitch: float :ivar yaw: Yaw (radian) :vartype yaw: float """ def __init__(self, x, y, z, roll, pitch, yaw): # X (meter) self.x = float(x) # Y (meter) self.y = float(y) # Z (meter) self.z = float(z) # Roll (radian) self.roll = float(roll) # Pitch (radian) self.pitch = float(pitch) # Yaw (radian) self.yaw = float(yaw) def __str__(self): position = "x = {:.4f}, y = {:.4f}, z = {:.4f}".format(self.x, self.y, self.z) orientation = "roll = {:.3f}, pitch = {:.3f}, yaw = {:.3f}".format(self.roll, self.pitch, self.yaw) return position + "\n" + orientation def __repr__(self): return self.__str__() def __add__(self, other): x = self.x + other.x y = self.y + other.y z = self.z + other.z roll = self.roll + other.roll pitch = self.pitch + other.pitch yaw = self.yaw + other.yaw return PoseObject(x, y, z, roll, pitch, yaw) def __sub__(self, other): x = self.x - other.x y = self.y - other.y z = self.z - other.z roll = self.roll - other.roll pitch = self.pitch - other.pitch yaw = self.yaw - other.yaw return PoseObject(x, y, z, roll, pitch, yaw) def __eq__(self, other): return self.x == other.x and self.y == other.y and self.z == other.z \ and self.roll == other.roll and self.pitch == other.pitch and self.yaw == other.yaw
[docs] def copy_with_offsets(self, x_offset=0., y_offset=0., z_offset=0., roll_offset=0., pitch_offset=0., yaw_offset=0.): """ Create a new pose from copying from copying actual pose with offsets :rtype: PoseObject """ return PoseObject(self.x + x_offset, self.y + y_offset, self.z + z_offset, self.roll + roll_offset, self.pitch + pitch_offset, self.yaw + yaw_offset)
[docs] def to_list(self): """ Return a list [x, y, z, roll, pitch, yaw] corresponding to the pose's parameters :rtype: list[float] """ list_pos = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw] return list(map(float, list_pos))
@property def quaternion(self): """ Return the quaternion in a list [qx, qy, qz, qw] :return: quaternion [qx, qy, qz, qw] :rtype: list[float, float, float, float] """ return self.euler_to_quaternion(self.roll, self.pitch, self.yaw) @property def quaternion_pose(self): """ Return the position and the quaternion in a list [x, y, z, qx, qy, qz, qw] :return: position [x, y, z] + quaternion [qx, qy, qz, qw] :rtype: list[float, float, float, float, float, float, float] """ return [self.x, self.y, self.z] + list(self.euler_to_quaternion(self.roll, self.pitch, self.yaw))
[docs] @staticmethod def euler_to_quaternion(roll, pitch, yaw): """ Convert euler angles to quaternion :param roll: roll in radians :type roll: float :param pitch: pitch in radians :type pitch: float :param yaw: yaw in radians :type yaw: float :return: quaternion in a list [qx, qy, qz, qw] :rtype: list[float, float, float, float] """ qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(roll / 2) * np.sin(pitch / 2) * np.sin( yaw / 2) qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.cos(pitch / 2) * np.sin( yaw / 2) qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(roll / 2) * np.sin(pitch / 2) * np.cos( yaw / 2) qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(roll / 2) * np.sin(pitch / 2) * np.sin( yaw / 2) return [qx, qy, qz, qw]
[docs] @staticmethod def quaternion_to_euler_angle(qx, qy, qz, qw): """ Convert euler angles to quaternion :param qx: :type qx: float :param qy: :type qy: float :param qz: :type qz: float :param qw: :type qw: float :return: euler angles in a list [roll, pitch, yaw] :rtype: list[float, float, float] """ ysqr = qy * qy t0 = +2.0 * (qw * qx + qy * qz) t1 = +1.0 - 2.0 * (qx * qx + ysqr) roll = np.arctan2(t0, t1) t2 = +2.0 * (qw * qy - qz * qx) t2 = np.clip(t2, a_min=-1.0, a_max=1.0) pitch = np.arcsin(t2) t3 = +2.0 * (qw * qz + qx * qy) t4 = +1.0 - 2.0 * (ysqr + qz * qz) yaw = np.arctan2(t3, t4) return roll, pitch, yaw
class TrajectoryObject(): def __init__(self, name, description, joint_names, points, accelerations, velocities, time_from_start): self.name = name self.description = description self.joint_names = joint_names self.points = points self.accelerations = accelerations self.velocities = velocities self.time_from_start = time_from_start