From 11a590ad0bbfe87b8e28b3473da84f902e09fbce Mon Sep 17 00:00:00 2001 From: Andrew Lui Date: Sun, 7 Jul 2024 14:02:37 +1000 Subject: [PATCH] the documentation is updated with addition of the new function move_and_rotate. --- arm_commander/commander_moveit.py | 36 +++++++++++++++---------------- docs/source/API_SUMMARY.md | 1 + docs/source/TUTORIAL_PART1.md | 6 ++++-- 3 files changed, 23 insertions(+), 20 deletions(-) diff --git a/arm_commander/commander_moveit.py b/arm_commander/commander_moveit.py index 9798bb6..45c4727 100644 --- a/arm_commander/commander_moveit.py +++ b/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 diff --git a/docs/source/API_SUMMARY.md b/docs/source/API_SUMMARY.md index 50dbea0..50bb774 100644 --- a/docs/source/API_SUMMARY.md +++ b/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. diff --git a/docs/source/TUTORIAL_PART1.md b/docs/source/TUTORIAL_PART1.md index ed21f32..8b00978 100644 --- a/docs/source/TUTORIAL_PART1.md +++ b/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).