the documentation is updated with addition of the new function move_and_rotate.

Author: Andrew Lui

arm_commander/commander_moveit.py

@@ -327,11 +327,11 @@ def current_joint_positons_as_list(self, print=False) -> list:
         return joint_values        
 
     # returns the pose (as xyzq list of 7 floats) of a link (link_name) in a frame of reference (reference_frame)
-    def pose_in_frame_as_xyzq(self, query_frame:str=None, reference_frame:str=None, ros_time:rospy.Time=None, print=False) -> list:
+    def pose_in_frame_as_xyzq(self, query_link:str=None, reference_frame:str=None, ros_time:rospy.Time=None, print=False) -> list:
         """ Get the pose of a link as a list of 7 floats (xyzq)
 
-        :param query_frame: The name of the frame to be queried, defaults to the end-effector
-        :type query_frame: str, optional
+        :param query_link: The name of the frame to be queried, defaults to the end-effector
+        :type query_link: str, optional
         :param reference_frame: The name of the frame of reference, defaults to the world/default
         :type reference_frame: str, optional
         :param ros_time: The time when the pose is queried, defaults to current time
@@ -342,19 +342,19 @@ def pose_in_frame_as_xyzq(self, query_frame:str=None, reference_frame:str=None,
         :return: The pose in the format of a list of 7 floats (xyzq)
         :rtype: list
         """
-        query_frame = self.END_EFFECTOR_LINK if query_frame is None else query_frame
-        pose = self.pose_in_frame(query_frame, reference_frame, ros_time)
+        query_link = self.END_EFFECTOR_LINK if query_link is None else query_link
+        pose = self.pose_in_frame(query_link, reference_frame, ros_time)
         xyzq = pose_tools.pose_to_xyzq(pose.pose)
         if print: 
-            rospy.loginfo(f'Frame {query_frame} tranformed into {reference_frame}: {xyzq} ')
+            rospy.loginfo(f'Frame {query_link} tranformed into {reference_frame}: {xyzq} ')
         return xyzq
 
     # returns the pose (as xyzrpy list of 6 floats) of a link (link_name) in a frame of reference (reference_frame)    
-    def pose_in_frame_as_xyzrpy(self, query_frame:str=None, reference_frame:str=None, ros_time:rospy.Time=None, print=False) -> list:
+    def pose_in_frame_as_xyzrpy(self, query_link:str=None, reference_frame:str=None, ros_time:rospy.Time=None, print=False) -> list:
         """ Get the pose of a link as a list of 6 floats (xyzrpy)
 
-        :param query_frame: The name of the frame to be queried, defaults to the end-effector
-        :type query_frame: str, optional
+        :param query_link: The name of the frame to be queried, defaults to the end-effector
+        :type query_link: str, optional
         :param reference_frame: The name of the frame of reference, defaults to the world/default
         :type reference_frame: str, optional
         :param ros_time: The time when the pose is queried, defaults to current time
@@ -365,19 +365,19 @@ def pose_in_frame_as_xyzrpy(self, query_frame:str=None, reference_frame:str=None
         :return: The pose in the format of a list of 6 floats (xyzrpy)
         :rtype: list
         """
-        query_frame = self.END_EFFECTOR_LINK if query_frame is None else query_frame
-        pose = self.pose_in_frame(query_frame, reference_frame, ros_time)
+        query_link = self.END_EFFECTOR_LINK if query_link is None else query_link
+        pose = self.pose_in_frame(query_link, reference_frame, ros_time)
         xyzrpy = pose_tools.pose_to_xyzrpy(pose.pose)
         if print: 
-            rospy.loginfo(f'Frame {query_frame} tranformed into {reference_frame}: {xyzrpy} ')
+            rospy.loginfo(f'Frame {query_link} tranformed into {reference_frame}: {xyzrpy} ')
         return xyzrpy
 
     # returns the pose (as PoseStamped) of a link (link_name) in a frame of reference (reference_frame)
-    def pose_in_frame(self, query_frame:str=None, reference_frame:str=None, ros_time:rospy.Time=None, print:bool=False) -> PoseStamped:
+    def pose_in_frame(self, query_link:str=None, reference_frame:str=None, ros_time:rospy.Time=None, print:bool=False) -> PoseStamped:
         """ Get the pose of a link as a PoseStamped
 
-        :param query_frame: The name of the input frame that is to be transformed into the reference frame, default to end-effector
-        :type query_frame: str, optional
+        :param query_link: The name of the input frame that is to be transformed into the reference frame, default to end-effector
+        :type query_link: str, optional
         :param reference_frame: The name of the frame of reference, defaults to the world/default
         :type reference_frame: str, optional
         :param ros_time: The time when the pose is queried, defaults to current time
@@ -386,18 +386,18 @@ def pose_in_frame(self, query_frame:str=None, reference_frame:str=None, ros_time
         :return: The pose as a PoseStamped
         :rtype: PoseStamped
         """
-        query_frame = self.END_EFFECTOR_LINK if query_frame is None else query_frame
+        query_link = self.END_EFFECTOR_LINK if query_link is None else query_link
         reference_frame = self.WORLD_REFERENCE_LINK if reference_frame is None else reference_frame     
         # reference frame is the target frame to which the query frame is to be transformed
         transform:TransformStamped
         try:
-            transform = self.tf_buffer.lookup_transform_full(reference_frame, rospy.Time(), query_frame, rospy.Time(), self.WORLD_REFERENCE_LINK, rospy.Duration(0.5))
+            transform = self.tf_buffer.lookup_transform_full(reference_frame, rospy.Time(), query_link, rospy.Time(), self.WORLD_REFERENCE_LINK, rospy.Duration(0.5))
             pose_stamped = pose_tools.transform_to_pose_stamped(transform)
         except Exception as e:
             rospy.logerr(f'Invalid parameter or TF error: {e}')
             raise
         if print: 
-            rospy.loginfo(f'Frame {query_frame} tranformed into {reference_frame}: {pose_stamped} ')
+            rospy.loginfo(f'Frame {query_link} tranformed into {reference_frame}: {pose_stamped} ')
         return pose_stamped
 
     # returns the pose (as Pose) of a link (link_name) of the default move_group planning frame

docs/source/API_SUMMARY.md

@@ -103,6 +103,7 @@ The `wait` parameter is another common feature that specifies the call is asynch
 | move_displacement | The dx, dy, and dz | cartesian path |
 | move_to_position | The target x, y, and z, the reference frame, and path planning | |
 | rotate_to_orientation | The target roll, pitch, and yaw, the reference frame, and path planning | |
+| move_and_rotate | The target x, y, z, roll, pitch, and yaw, the reference frame, and path planning | |
 | move_to_pose | Pose, PoseStamped, a 6-list (xyzrpy) or a 7-list (xyzqqqq) | |
 
 The following functions accepts multiple positions or poses in one move command. The path is always cartesian.

docs/source/TUTORIAL_PART1.md

@@ -177,7 +177,6 @@ arm_commander.move_to_position(x = 0.4, y = 0.2, wait=True)
 arm_commander.reset_state()
 ```
 
-
 ### Move using a displacement
 
 The function `move_displacement()` commands the robot arm to move based on a displacement from the current position. The following source code is from `simple_move_4.py`.  The end-effector will move 10 cm ten times.
@@ -197,6 +196,8 @@ The default values of the three component parameters are the current values.
 
 ### Move to Both Position and Orientation
 
+There are two functions for moving to a both specified position and orientation.  
+
 The function `move_pose()` commands the end-effector to move to a target pose, which can be of type `Pose` or `PoseStamped`. 
 ```python
 pose = Pose()
@@ -225,9 +226,10 @@ xyzrpy = [0.4, 0.0, 0.4, 3.14, 0.0, 0.6]
 arm_commander.move_to_pose(xyzrpy, wait=True)
 arm_commander.reset_state() 
 ```
-
 ![Animation of the Movement](../assets/ArmCommander-SimpleMove5.gif)
 
+However, the function `move_pose()` accepts only fully specified pose. On the other hand, the function `move_and_rotate()` supports partially specified xyzrpy, and the unspecified components are defaulted to the current values.
+
 ### Cartesian Movement
 
 The function `move_position()` supports both cartesian path planning (with collision avoidance) and algorithmic path planning (based on a path planner).