def to_tree(cls, node: Rotation, ctx): """ Convert an instance of the 'Dimension' type into YAML representations. Parameters ---------- node : Instance of the 'Dimension' type to be serialized. ctx : An instance of the 'AsdfFile' object that is being written out. Returns ------- A basic YAML type ('dict', 'list', 'str', 'int', 'float', or 'complex') representing the properties of the 'Dimension' type to be serialized. """ tree = {} if not hasattr(node, "wx_meta"): # default to quaternion representation tree["quaternions"] = node.as_quat() elif node.wx_meta["constructor"] == "from_quat": tree["quaternions"] = node.as_quat() elif node.wx_meta["constructor"] == "from_matrix": tree["matrix"] = node.as_matrix() elif node.wx_meta["constructor"] == "from_rotvec": tree["rotvec"] = node.as_rotvec() elif node.wx_meta["constructor"] == "from_euler": seq_str = node.wx_meta["seq"] if not len(seq_str) == 3: if all([c in "xyz" for c in seq_str]): seq_str = seq_str + "".join( [c for c in "xyz" if c not in seq_str]) elif all([c in "XYZ" for c in seq_str]): seq_str = seq_str + "".join( [c for c in "XYZ" if c not in seq_str]) else: # pragma: no cover raise ValueError( "Mix of intrinsic and extrinsic euler angles.") angles = node.as_euler(seq_str, degrees=node.wx_meta["degrees"]) angles = np.squeeze(angles[..., :len(node.wx_meta["seq"])]) if node.wx_meta["degrees"]: angles = Q_(angles, "degree") else: angles = Q_(angles, "rad") tree["sequence"] = node.wx_meta["seq"] tree["angles"] = angles else: # pragma: no cover raise NotImplementedError("unknown or missing constructor") return tree
def get_state(self): if not self.state_q.empty(): state = self.state_q.get() quaternion = lambda o: np.array([o.x, o.y, o.z, o.w]) rot_mat = R.from_quat(quaternion(state['pose'].orientation)) x = np.array([ state['pose'].position.x, state['pose'].position.y, R.as_euler(rot_mat, 'zyx')[0] ]) xdot = np.array([ state['twist'].linear.x, state['twist'].linear.y, state['twist'].angular.z ]) self.cur_state = [x, xdot] return self.cur_state[0], self.cur_state[1]
def to_yaml_tree(self, obj: Rotation, tag: str, ctx) -> dict: """Convert to python dict.""" tree = {} if not hasattr(obj, ROT_META): # default to quaternion representation tree["quaternions"] = obj.as_quat() elif getattr(obj, ROT_META)["constructor"] == "from_quat": tree["quaternions"] = obj.as_quat() elif getattr(obj, ROT_META)["constructor"] == "from_matrix": tree["matrix"] = obj.as_matrix() elif getattr(obj, ROT_META)["constructor"] == "from_rotvec": tree["rotvec"] = obj.as_rotvec() elif getattr(obj, ROT_META)["constructor"] == "from_euler": seq_str = getattr(obj, ROT_META)["seq"] if not len(seq_str) == 3: if all(c in "xyz" for c in seq_str): seq_str = seq_str + "".join( [c for c in "xyz" if c not in seq_str]) elif all(c in "XYZ" for c in seq_str): seq_str = seq_str + "".join( [c for c in "XYZ" if c not in seq_str]) else: # pragma: no cover raise ValueError( "Mix of intrinsic and extrinsic euler angles.") angles = obj.as_euler(seq_str, degrees=getattr(obj, ROT_META)["degrees"]) angles = np.squeeze( angles[..., :len(getattr(obj, ROT_META)["seq"])]) if getattr(obj, ROT_META)["degrees"]: angles = Q_(angles, "degree") else: angles = Q_(angles, "rad") tree["sequence"] = getattr(obj, ROT_META)["seq"] tree["angles"] = angles else: # pragma: no cover raise NotImplementedError("unknown or missing constructor") return tree
plt.plot(demo_qs[1], label='y', color='r', linestyle='-') plt.plot(q[1], color='r', linestyle='--') plt.plot(demo_qs[2], label='z', color='g', linestyle='-') plt.plot(q[2], color='g', linestyle='--') plt.plot(demo_qs[3], label='w', color='m', linestyle='-') plt.plot(q[3], color='m', linestyle='--') plt.legend() plt.title('quat demo and dmp \nstart=' + str(demo_qs[:, 0]) + '\n end=' + str(demo_qs[:, -1])) plt.show() # also do euler conversion for more easily interpretable units demo_rots = R.from_quat(demo_qs.T) euler_demo = R.as_euler(demo_rots, 'XYZ').T dmp_rots = R.from_quat(q.T) euler_roll = R.as_euler(dmp_rots, 'XYZ').T plt.plot(euler_demo[0], label='euler x', color='b', linestyle='-') plt.plot(euler_roll[0], color='b', linestyle='--') plt.plot(euler_demo[1], label='euler y', color='r', linestyle='-') plt.plot(euler_roll[1], color='r', linestyle='--') plt.plot(euler_demo[2], label='euler z', color='g', linestyle='-') plt.plot(euler_roll[2], color='g', linestyle='--') plt.legend() plt.title('euler demo and dmp \n start=' + str(euler_demo[:, 0]) + '\n end=' + str(euler_demo[:, -1])) plt.show()
def strRotation(r: R) -> list: return r.as_euler("ZYX", degrees=True)
def _handle_pose(self, msg): if self._tf_buffer.can_transform("bluerov2/base_link", "map_ned", rospy.Time.now(), rospy.Duration.from_sec(0.5)): tf = self._tf_buffer.lookup_transform("bluerov2/base_link", "map_ned", rospy.Time.now(), rospy.Duration.from_sec(0.5)) else: return # TODO neptus gets confused if you try to give all available estimates, there is probably a transformation problem # self._estimated_state_msg.x = tf.transform.translation.x # self._estimated_state_msg.y = tf.transform.translation.y # self._estimated_state_msg.z = tf.transform.translation.z # self._estimated_state_msg.depth = tf.transform.translation.z # self._estimated_state_msg.height = msg.pose.pose.position.z R = Rotation([ tf.transform.rotation.x, tf.transform.rotation.y, tf.transform.rotation.z, tf.transform.rotation.w ]) self._estimated_state_msg.phi, self._estimated_state_msg.theta, self._estimated_state_msg.psi = R.as_euler( "xyz", False).squeeze()