cheng
110 lines
3.0 KiB
Python
110 lines
3.0 KiB
Python
#!/usr/bin/env python
|
|
|
|
"""utility.py
|
|
|
|
Utility methods.
|
|
|
|
Import this module as `import utility` and use functions through the module namespace,
|
|
for example `utility.quat2eulerZYX(...)` and `utility.primitive_state_true_for_groups(...)`.
|
|
"""
|
|
|
|
__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
|
|
__author__ = "Flexiv"
|
|
|
|
import math
|
|
|
|
# pip install scipy
|
|
from scipy.spatial.transform import Rotation as R
|
|
|
|
|
|
def quat2eulerZYX(quat, degree=False):
|
|
"""
|
|
Convert quaternion to Euler angles with ZYX axis rotations.
|
|
|
|
Parameters
|
|
----------
|
|
quat : float list
|
|
Quaternion input in [w,x,y,z] order.
|
|
degree : bool
|
|
Return values in degrees, otherwise in radians.
|
|
|
|
Returns
|
|
----------
|
|
float list
|
|
Euler angles in [x,y,z] order, radian by default unless specified otherwise.
|
|
Returned angles are normalized by scipy to a principal range. In degree mode,
|
|
this is typically within [-180, 180].
|
|
"""
|
|
|
|
# Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation
|
|
# NOTE: scipy uses [x,y,z,w] order to represent quaternion. The returned Euler values
|
|
# are a normalized representation of the same physical orientation.
|
|
eulerZYX = (
|
|
R.from_quat([quat[1], quat[2], quat[3], quat[0]])
|
|
.as_euler("xyz", degrees=degree)
|
|
.tolist()
|
|
)
|
|
|
|
return eulerZYX
|
|
|
|
|
|
def primitive_state_true_for_groups(primitive_states, state_name):
|
|
"""
|
|
Check whether primitive states are true for all groups included in primitive_states.
|
|
|
|
Parameters
|
|
----------
|
|
primitive_states : dict
|
|
Primitive states keyed by joint group.
|
|
state_name : str or dict
|
|
Primitive state name to check for all groups, or per-group state names keyed by joint
|
|
group, e.g. "reachedTarget" or {group: "terminated"}.
|
|
|
|
Returns
|
|
----------
|
|
bool
|
|
True if all included groups contain the requested primitive state and its integer value is
|
|
non-zero.
|
|
"""
|
|
|
|
def primitive_state_true(primitive_state, requested_state_name):
|
|
if requested_state_name not in primitive_state.names_and_values:
|
|
return False
|
|
|
|
value = primitive_state.names_and_values[requested_state_name]
|
|
return isinstance(value, int) and value != 0
|
|
|
|
if isinstance(state_name, dict):
|
|
return all(
|
|
group in state_name
|
|
and primitive_state_true(primitive_state, state_name[group])
|
|
for group, primitive_state in primitive_states.items()
|
|
)
|
|
|
|
return all(
|
|
primitive_state_true(primitive_state, state_name)
|
|
for primitive_state in primitive_states.values()
|
|
)
|
|
|
|
|
|
def list2str(ls):
|
|
"""
|
|
Convert a list to a string.
|
|
|
|
Parameters
|
|
----------
|
|
ls : list
|
|
Source list of any size.
|
|
|
|
Returns
|
|
----------
|
|
str
|
|
A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value
|
|
followed by a space, including the last one.
|
|
"""
|
|
|
|
ret_str = ""
|
|
for i in ls:
|
|
ret_str += str(i) + " "
|
|
return ret_str
|