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

@@ -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

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.

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]

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