From eea35b5ac412e9d9a74ae56b0dff92f611833938 Mon Sep 17 00:00:00 2001 From: Iori Yanokura Date: Mon, 7 Oct 2024 23:18:10 +0900 Subject: [PATCH] Added quaternion_xyzw method to Coordinates class Introduced quaternion_xyzw method to return quaternion in [x, y, z, w] format, complementing the existing quaternion method, which uses [w, x, y, z] format. Added the option to specify input_quaternion_order in the Coordinates constructor, allowing users to input quaternions in either wxyz or xyzw order. --- skrobot/coordinates/base.py | 50 ++++++++++++++++++++++++++++-- skrobot/interfaces/_pybullet.py | 5 ++- skrobot/interfaces/ros/tf_utils.py | 4 +-- skrobot/planner/utils.py | 2 +- 4 files changed, 53 insertions(+), 8 deletions(-) diff --git a/skrobot/coordinates/base.py b/skrobot/coordinates/base.py index 0930aef6..7fed81ce 100644 --- a/skrobot/coordinates/base.py +++ b/skrobot/coordinates/base.py @@ -22,6 +22,8 @@ from skrobot.coordinates.math import rotation_matrix from skrobot.coordinates.math import rpy2quaternion from skrobot.coordinates.math import rpy_angle +from skrobot.coordinates.math import wxyz2xyzw +from skrobot.coordinates.math import xyzw2wxyz def transform_coords(c1, c2, out=None): @@ -196,7 +198,8 @@ def __init__(self, rot=None, name=None, hook=None, - check_validity=True): + check_validity=True, + input_quaternion_order='wxyz'): if check_validity: if (isinstance(pos, list) or isinstance(pos, np.ndarray)): T = np.array(pos, dtype=np.float64) @@ -206,6 +209,16 @@ def __init__(self, if rot is None: self._rotation = np.eye(3) else: + if input_quaternion_order == 'wxyz': + pass + elif input_quaternion_order == 'xyzw': + if np.array(rot).shape == (4,): + rot = xyzw2wxyz(rot) + else: + msg = "Invalid input_quaternion_order: " + msg += "{}. Must be 'wxyz' or 'xyzw'.".format( + input_quaternion_order) + raise ValueError(msg) self.rotation = rot if pos is None: self._translation = np.array([0, 0, 0]) @@ -602,7 +615,7 @@ def T(self): @property def quaternion(self): - """Property of quaternion + """Property of quaternion in [w, x, y, z] format Returns ------- @@ -622,6 +635,39 @@ def quaternion(self): """ return matrix2quaternion(self._rotation) + @property + def quaternion_wxyz(self): + """Property of quaternion in [w, x, y, z] format + + Returns + ------- + q : numpy.ndarray + [w, x, y, z] quaternion + """ + return matrix2quaternion(self._rotation) + + @property + def quaternion_xyzw(self): + """Property of quaternion in [x, y, z, w] format + + Returns + ------- + q : numpy.ndarray + [x, y, z, w] quaternion + + Examples + -------- + >>> from numpy import pi + >>> from skrobot.coordinates import make_coords + >>> c = make_coords() + >>> c.quaternion_xyzw + array([0., 0., 0., 1.]) + >>> c.rotate(pi / 3, 'y').rotate(pi / 5, 'z') + >>> c.quaternion_xyzw + array([0.1545085 , 0.47552826, 0.26761657, 0.8236391 ]) + """ + return wxyz2xyzw(matrix2quaternion(self._rotation)) + @property def dual_quaternion(self): """Property of DualQuaternion diff --git a/skrobot/interfaces/_pybullet.py b/skrobot/interfaces/_pybullet.py index 9a7dd076..ced01101 100644 --- a/skrobot/interfaces/_pybullet.py +++ b/skrobot/interfaces/_pybullet.py @@ -4,7 +4,6 @@ import numpy as np from skrobot.coordinates import Coordinates -from skrobot.coordinates.math import wxyz2xyzw from skrobot.coordinates.math import xyzw2wxyz from skrobot.coordinates import matrix2quaternion from skrobot.coordinates import quaternion2rpy @@ -85,7 +84,7 @@ def __init__(self, robot, urdf_path=None, use_fixed_base=False, except Exception as e: print(e) self.robot_id = p.loadURDF(urdf_path, self.translation, - wxyz2xyzw(self.quaternion), + self.quaternion_xyzw, useFixedBase=use_fixed_base) self.load_bullet() @@ -125,7 +124,7 @@ def _reset_position_and_orientation(self): This function is wrapper of pybullet.resetBasePositionAndOrientation. """ p.resetBasePositionAndOrientation(self.robot_id, self.translation, - wxyz2xyzw(self.quaternion)) + self.quaternion_xyzw) def translate(self, vec, wrt='local'): """Translate robot in simulator. diff --git a/skrobot/interfaces/ros/tf_utils.py b/skrobot/interfaces/ros/tf_utils.py index aff74f87..786d1923 100644 --- a/skrobot/interfaces/ros/tf_utils.py +++ b/skrobot/interfaces/ros/tf_utils.py @@ -20,7 +20,7 @@ def coords_to_geometry_pose(coords): pose.position.x = coords.translation[0] pose.position.y = coords.translation[1] pose.position.z = coords.translation[2] - q = coords.quaternion + q = coords.quaternion_wxyz pose.orientation.w = q[0] pose.orientation.x = q[1] pose.orientation.y = q[2] @@ -45,7 +45,7 @@ def coords_to_tf_pose(coords): pose.translation.x = coords.translation[0] pose.translation.y = coords.translation[1] pose.translation.z = coords.translation[2] - q = coords.quaternion + q = coords.quaternion_wxyz pose.rotation.w = q[0] pose.rotation.x = q[1] pose.rotation.y = q[2] diff --git a/skrobot/planner/utils.py b/skrobot/planner/utils.py index f619b268..abb75f28 100644 --- a/skrobot/planner/utils.py +++ b/skrobot/planner/utils.py @@ -169,7 +169,7 @@ def _forward_kinematics(robot_model, link_list = [joint.child_link for joint in joint_list] ef_pos_wrt_world = move_target.worldpos() - ef_quat_wrt_world = move_target.worldcoords().quaternion + ef_quat_wrt_world = move_target.worldcoords().quaternion_wxyz world_coordinate = CascadedCoords() def quaternion_kinematic_matrix(q):