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()
