From 753f9e1bbdb596f3af2b01cd2e9e616614bc31f9 Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Tue, 12 Dec 2023 17:23:40 +0100 Subject: [PATCH] refactor(moveit server): improve codebase (#194) This commit improves the codebase and fixes some small bugs in the panda moveit server. --- panda_gazebo/launch/put_robot_in_world.launch | 3 +- panda_gazebo/nodes/panda_moveit_server.py | 2 +- .../src/panda_gazebo/core/moveit_server.py | 1619 ++++++++--------- .../tests/manual/moveit_commander_test.py | 125 +- .../tests/manual/panda_moveit_server_test.py | 98 +- 5 files changed, 836 insertions(+), 1011 deletions(-) mode change 100644 => 100755 panda_gazebo/tests/manual/moveit_commander_test.py mode change 100644 => 100755 panda_gazebo/tests/manual/panda_moveit_server_test.py diff --git a/panda_gazebo/launch/put_robot_in_world.launch b/panda_gazebo/launch/put_robot_in_world.launch index bb5c8a64..b4d5cc39 100644 --- a/panda_gazebo/launch/put_robot_in_world.launch +++ b/panda_gazebo/launch/put_robot_in_world.launch @@ -72,7 +72,7 @@ - + @@ -84,6 +84,7 @@ + diff --git a/panda_gazebo/nodes/panda_moveit_server.py b/panda_gazebo/nodes/panda_moveit_server.py index b0e62187..10835379 100755 --- a/panda_gazebo/nodes/panda_moveit_server.py +++ b/panda_gazebo/nodes/panda_moveit_server.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 """This node sets up several extra services that can be used to control the Panda Emika -Franka robot using the Moveit framework. +Franka robot using the MoveIt framework. Source code ----------- diff --git a/panda_gazebo/src/panda_gazebo/core/moveit_server.py b/panda_gazebo/src/panda_gazebo/core/moveit_server.py index 067bc9fa..00845af0 100644 --- a/panda_gazebo/src/panda_gazebo/core/moveit_server.py +++ b/panda_gazebo/src/panda_gazebo/core/moveit_server.py @@ -27,16 +27,14 @@ 'panda_moveit_planner_server/max_velocity_scaling_factor' and 'panda_moveit_planner_server/max_velocity_scaling_factor' topics. """ -import copy import re import sys -from itertools import compress import moveit_commander import numpy as np import rospy from dynamic_reconfigure.server import Server -from geometry_msgs.msg import Pose, PoseStamped, Quaternion +from geometry_msgs.msg import Pose, PoseStamped from moveit_commander.exception import MoveItCommanderException from moveit_msgs.msg import DisplayTrajectory from rospy.exceptions import ROSException @@ -149,13 +147,12 @@ def __init__( # noqa: C901 "Shutting down '%s' because robot_description was not found." % rospy.get_name() ) - ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1) else: err_msg = "Shutting down '%s' because %s" % ( rospy.get_name(), e.args[0], ) - ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1) + ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1) # Initialise group commanders. try: @@ -171,13 +168,12 @@ def __init__( # noqa: C901 arm_move_group, ) ) - ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1) else: err_msg = "Shutting down '%s' because %s" % ( rospy.get_name(), e.args[0], ) - ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1) + ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1) if self._load_gripper: try: rospy.logdebug("Initialise MoveIt Panda hand commander.") @@ -197,8 +193,9 @@ def __init__( # noqa: C901 e.args[0], ) ros_exit_gracefully(shutdown_msg=err_msg, exit_code=1) - self.move_group_arm.set_end_effector_link(arm_ee_link) + + # Create display trajectory publisher. self._display_trajectory_publisher = rospy.Publisher( "move_group/display_planned_path", DisplayTrajectory, @@ -352,11 +349,19 @@ def __init__( # noqa: C901 "Current joint_states not ready yet, retrying for getting " "'joint_states'." ) + arm_joints = self.move_group_arm.get_active_joints() + hand_joints = ( + self.move_group_hand.get_active_joints() if self._load_gripper else [] + ) + controlled_joints = ( + [arm_joints, hand_joints] + if self._joint_states.name[0] in arm_joints + else [hand_joints, arm_joints] + ) self._controlled_joints_dict = { - "arm": flatten_list(self.move_group_arm.get_active_joints()), - "hand": flatten_list(self.move_group_hand.get_active_joints()) - if self._load_gripper - else [], + "arm": arm_joints, + "hand": hand_joints, + "both": flatten_list(controlled_joints), } # Retrieve joint state mask. @@ -384,16 +389,12 @@ def _get_params(self): 'panda_moveit_planner_server/max_velocity_scaling_factor' and 'panda_moveit_planner_server/max_acceleration_scaling_factor' topics. """ - try: - self._max_velocity_scaling = rospy.get_param("~max_velocity_scaling_factor") - except KeyError: - self._max_velocity_scaling = None - try: - self._max_acceleration_scaling = rospy.get_param( - "~max_acceleration_scaling_factor" - ) - except KeyError: - self._max_acceleration_scaling = None + self._max_velocity_scaling = rospy.get_param( + "~max_velocity_scaling_factor", None + ) + self._max_acceleration_scaling = rospy.get_param( + "~max_acceleration_scaling_factor", None + ) def _link_exists(self, link_name): """Function checks whether a given link exists in the robot_description. @@ -406,8 +407,53 @@ def _link_exists(self, link_name): """ return link_name in self.robot.get_link_names() + def _split_positions_moveit_setpoint(self, positions_moveit_setpoint): + """Splits a combined MoveIt arm and hand joint position setpoint dictionary into + separate dictionaries for each control group. + + Args: + positions_moveit_setpoint (dict): Dictionary that contains the combined arm + and hand joint positions setpoint. + + Returns: + dict: Dictionary containing dictionaries for arm and hand joint positions + setpoint respectively. + """ + return { + "arm": { + joint_name: joint_position + for joint_name, joint_position in positions_moveit_setpoint.items() + if joint_name in self._controlled_joints_dict["arm"] + }, + "hand": { + joint_name: joint_position + for joint_name, joint_position in positions_moveit_setpoint.items() + if joint_name in self._controlled_joints_dict["hand"] + }, + } + + def _set_joint_value_target(self, group, positions): + """Sets the joint value target for a control group. + + Args: + group (str): The control group ("arm" or "hand"). + positions (dict): The joint positions. + + Returns: + bool: True if successful, False otherwise. + str: The error message, or an empty string if successful. + """ + try: + rospy.logdebug( + f"{group.capitalize()} joint positions setpoint: {positions}" + ) + getattr(self, f"move_group_{group}").set_joint_value_target(positions) + return True, "" + except MoveItCommanderException as e: + return False, e.args[0] + def _execute(self, control_group="both", wait=True): # noqa: C901 - """Plan and execute a trajectory/pose or orientation setpoints + """Plan and execute a trajectory/pose or orientation setpoints. Args: control_group (str, optional): The robot control group for which you want @@ -420,341 +466,443 @@ def _execute(self, control_group="both", wait=True): # noqa: C901 If ``control_group == "both"`` then ``["arm_success", "hand_success"]``. """ control_group = control_group.lower() + if control_group not in ["arm", "hand", "both"]: + rospy.logwarn( + f"Control group '{control_group}' does not exist. Please specify a " + "valid control group. Valid values are 'arm', 'hand' or 'both'." + ) + return [False] - # Execute. - retval = [] - if control_group == "arm": - (arm_plan_retval, _, _, error_code) = self.move_group_arm.plan() - if arm_plan_retval: - arm_retval = self.move_group_arm.go(wait=wait) - if wait: - self.move_group_arm.stop() - else: - rospy.logwarn( - "No plan found for the current arm setpoints since '%s'" - % translate_moveit_error_code(error_code) - ) - arm_retval = False - retval.append(arm_retval) - elif control_group == "hand": - if self._load_gripper: - (hand_plan_retval, _, _, error_code) = self.move_group_hand.plan() - if hand_plan_retval: - hand_retval = self.move_group_hand.go(wait=wait) - if wait: - self.move_group_hand.stop() - else: - rospy.logwarn( - "No plan found for the current hand setpoints since '%s'" - % translate_moveit_error_code(error_code) - ) - hand_retval = False - retval.append(hand_retval) - else: + # Plan and execute trajectory. + retval, groups = [], [] + if control_group in ["arm", "both"]: + groups.append(self.move_group_arm) + if control_group in ["hand", "both"]: + groups.append(self.move_group_hand if self._load_gripper else None) + for group in groups: + if group is None: rospy.logwarn("Hand commands not executed since gripper is not loaded.") retval.append(False) - elif control_group == "both": - # Get hand plan. - if not self._load_gripper: - rospy.logwarn_once( - "Hand commands not executed since gripper is not loaded." - ) - else: - (hand_plan_retval, _, _, error_code) = self.move_group_hand.plan() - - # Get arm plan. - (arm_plan_retval, _, _, error_code) = self.move_group_arm.plan() + continue - # Execute arm/hand plans - if self._load_gripper: - if hand_plan_retval: - hand_retval = self.move_group_hand.go(wait=False) - else: - rospy.logwarn( - "No plan found for the current hand setpoints since '%s'" - % translate_moveit_error_code(error_code) - ) - hand_retval = False - retval.append(hand_retval) - if arm_plan_retval: - arm_retval = self.move_group_arm.go(wait=wait) + (plan_retval, _, _, error_code) = group.plan() + if plan_retval: + group_retval = group.go(wait=wait) if wait: - if self._load_gripper: - self.move_group_hand.stop() - self.move_group_arm.stop() + group.stop() else: rospy.logwarn( - "No plan found for the current hand setpoints since '%s'" - % translate_moveit_error_code(error_code) + f"No plan found for the current {group.get_name()} setpoints " + f"since '{translate_moveit_error_code(error_code)}'." ) - arm_retval = False - retval.append(arm_retval) - else: - logwarn_msg = ( - f"Control group '{control_group}' does not exist. Please specify a " - "valid control group. Valid values are 'arm', 'hand' or 'both'." - ) - rospy.logwarn(logwarn_msg) - retval = [False] + group_retval = False + retval.append(group_retval) + return retval - def _create_joint_positions_commands( # noqa: C901 - self, input_msg, control_group="both" + def _create_positions_moveit_setpoints( # noqa: C901 + self, set_joint_positions_req, control_group ): - """Converts the service input message in :mod:`moveit_commander` compatible joint - position setpoint commands. While doing this it also verifies whether the given - input message is valid. + """Converts a :class:`~panda_gazebo.srv.SetJointPositionsRequest` message into + a :class:`~moveit_commander.move_group.MoveGroupCommander` hand and/or arm joint + positions setpoint dictionary. Args: - input_msg (str): The service input message we want to validate. - control_group (str, optional): The robot control group for which you want to - execute the control. Options are ``arm`` or ``hand`` or ``both``. - Defaults to ``both``. + set_joint_positions_req (:class:`~panda_gazebo.srv.SetJointPositionsRequest`): # noqa: E501 + The service request message we want to convert. + control_group (str): The control group for which we want to create the joint + positions setpoint. Options are ``arm`` or ``hand`` or ``both``. Returns: - dict: Dictionary that contains the 'moveit_commander' arm and hand joint - position commands. Grouped by control group. + dict: Dictionary containing the arm and/or hand joint positions setpoint. Raises: - :obj:`panda_gazebo.exceptions.InputMessageInvalidError`: Raised when the - input_msg could not be converted into 'moveit_commander' arm hand joint - position commands. + :obj:`panda_gazebo.exceptions.InputMessageInvalidError`: Raised when no + joint position setpoint dictionary could be created from the given input + message. """ - joint_names = input_msg.joint_names - joint_positions = list(input_msg.joint_positions) - control_group = control_group.lower() + joint_names = set_joint_positions_req.joint_names + joint_positions = list(set_joint_positions_req.joint_positions) + controlled_joints = self._controlled_joints_dict[control_group] + control_group_str = "arm and hand" if control_group == "both" else control_group - # Validate control_group. - if control_group not in ["arm", "hand", "both"]: + # Raise error if no joint positions were given. + if len(joint_positions) == 0: + raise InputMessageInvalidError( + message=("No joint positions were given."), + log_message=( + "No joint positions were given. Please specify at least one joint " + "position." + ), + ) + + # Handle joint positions request without joint names. + controlled_joints_size = len(controlled_joints) + if len(joint_names) == 0: + # Display warning if to many or to few joint positions were given. + joint_positions_count = len(joint_positions) + if joint_positions_count != controlled_joints_size: + action = ( + "extra joint positions were ignored" + if joint_positions_count > controlled_joints_size + else ( + "missing joint positions were set to the current joint " + "positions" + ) + ) + rospy.logwarn( + f"You specified {joint_positions_count} joint positions while the " + f"Panda robot {control_group_str} contains " + f"{controlled_joints_size} active joints. As a result, the " + f"{action}." + ) + + # Split joint positions into arm and hand joint positions and return. + joint_positions_setpoint = dict(zip(controlled_joints, joint_positions)) + if control_group != "both": + return {control_group: joint_positions_setpoint} + return self._split_positions_moveit_setpoint(joint_positions_setpoint) + + # Check if duplicate joint names were given. + duplicate_joint_names = get_duplicate_list(joint_names) + if duplicate_joint_names: + rospy.logwarn( + f"You specified duplicate joint names ({duplicate_joint_names}). " + "As a result, the duplicate joint names are ignored." + ) + joint_names = get_unique_list(joint_names) + + # Apply gripper width to first hand joint. + # NOTE: Only the first joint is controlled directly by MoveIT. + if "gripper_width" in joint_names: + gripper_width_index = joint_names.index("gripper_width") + joint_positions[gripper_width_index] /= 2 + joint_names[gripper_width_index] = self._controlled_joints_dict["hand"][0] + + # Validate joint_names. + invalid_joint_names = [ + joint_name + for joint_name in joint_names + if joint_name not in controlled_joints + ] + if invalid_joint_names: + joint_names = [ + joint_name + for joint_name in joint_names + if joint_name not in invalid_joint_names + ] + joint_positions = [ + joint_position + for joint_position, joint_name in zip(joint_positions, joint_names) + if joint_name not in invalid_joint_names + ] + + # Raise error if no valid joint names were given and log warning if invalid + # joint names were given. + word_forms = ( + "Joint" if len(invalid_joint_names) == 1 else "Joints", + "was" if len(invalid_joint_names) == 1 else "were", + "it is" if len(invalid_joint_names) == 1 else "they are", + ) logwarn_msg = ( - f"The '{control_group}' control group does not exist. Please specify " - "a valid control group (options: 'arm', 'hand', 'both')." + f"{word_forms[0]} {invalid_joint_names} {word_forms[1]} ignored since " + f"{word_forms[2]} invalid. Valid joint names for controlling the panda " + f"{control_group_str} are {controlled_joints}." ) - rospy.logerr(logwarn_msg) - raise InputMessageInvalidError( - message=f"Control group '{control_group}' does not exist.", - log_message=logwarn_msg, + if len(joint_positions) == 0: + raise InputMessageInvalidError( + message=( + "No valid joint names were given. Please specify at least one " + "valid joint name." + ), + log_message=f"No valid joint names were given. {logwarn_msg}", + ) + rospy.logwarn(logwarn_msg) + + # Check if each joint name has a corresponding joint position. + if len(joint_names) != len(joint_positions): + joint_positions_count = len(joint_positions) + joint_names_count = len(joint_names) + joint_positions_str = ( + "joint position" if joint_positions_count == 1 else "joint positions" ) - elif control_group == "hand" and not self._load_gripper: + joint_names_str = "joint" if joint_names_count == 1 else "joints" logwarn_msg = ( - "The 'hand' control group can not be used when the 'load_gripper' " - "variable is set to False. Please change 'load_gripper' to True or " - "specify a valid control group (options: ['arm'])." + f"You specified {joint_positions_count} {joint_positions_str} while " + "the 'joint_names' field of the 'panda_gazebo/SetJointPositions' " + f"message contains {joint_names_count} {joint_names_str}. Please make " + "sure you supply a joint position for each joint contained in the " + "'joint_names' field." ) - rospy.logerr(logwarn_msg) raise InputMessageInvalidError( - message=f"Control group '{control_group}' does not exist.", + message=( + "Joint_names and joint_positions fields of the input message are " + "of different lengths." + ), log_message=logwarn_msg, + details={ + "joint_positions_command_length": joint_positions_count, + "joint_names_length": joint_names_count, + }, ) - elif control_group == "both" and not self._load_gripper: - # Change to arm if the hand is not loaded. - control_group = "arm" - # Get controlled joints, throw warning if control group is incorrect. - if control_group == "arm": - controlled_joints = self._controlled_joints_dict["arm"] - elif control_group == "hand": - controlled_joints = self._controlled_joints_dict["hand"] - else: - controlled_joints = flatten_list( + # Split joint positions into arm and hand joint positions and return. + joint_positions_setpoint = dict(zip(joint_names, joint_positions)) + if control_group != "both": + return {control_group: joint_positions_setpoint} + return self._split_positions_moveit_setpoint(joint_positions_setpoint) + + def _get_valid_joint_limits(self, joint_limits_names, joint_limits_values): + """Returns a dictionary containing the valid joint limits. + + Args: + joint_limits_names (list): List containing the joint limit names. + joint_limits_values (list): List containing the joint limit values. + + Returns: + dict: Dictionary containing the valid joint limits. + """ + # Check if limit names and limit values are of equal length. + joint_positions_command_length = len(joint_limits_values) + joint_names_length = len(joint_limits_names) + if joint_positions_command_length != joint_names_length: + rospy.logwarn( + "Joint limits ignored as the number of joints ({}) is " + "unequal to the number of limit values ({}).".format( + joint_names_length, joint_positions_command_length + ) + ) + return {} + + # Check if joint names are valid. + valid_joint_names = flatten_list( + [ + [joint + "_min", joint + "_max"] + for joint in self._controlled_joints_dict["both"] + ] + ) + invalid_joint_names = [ + name for name in joint_limits_names if name not in valid_joint_names + ] + if len(invalid_joint_names) != 0: + singular = len(invalid_joint_names) == 1 + warn_strings = ( + ( + "limit", + invalid_joint_names[0], + "was", + "it is not a valid joint limit", + ) + if singular + else ( + "limits", + ", ".join(invalid_joint_names), + "were", + "they are not valid joint limits", + ) + ) + rospy.logwarn( + f"Joint {warn_strings[0]} '{warn_strings[1]}' " + f"{warn_strings[2]} ignored since {warn_strings[3]}. Valid " + f"values are '{valid_joint_names}'." + ) + joint_limits_names = [ + joint_name + for joint_name in joint_limits_names + if joint_name not in invalid_joint_names + ] + joint_limits_values = [ + joint_limit + for joint_limit_name, joint_limit in zip( + joint_limits_names, + joint_limits_values, + ) + if joint_limit_name not in invalid_joint_names + ] + + # Ensure that joint limits have a min and max value. + limited_joints = get_unique_list( + [ + name.replace("_min", "").replace("_max", "") + for name in joint_limits_names + ] + ) + required_joint_limits = flatten_list( + [[joint + "_min", joint + "_max"] for joint in limited_joints] + ) + missing_joint_limits = [ + required_joint_limit + for required_joint_limit in required_joint_limits + if required_joint_limit not in joint_limits_names + ] + if missing_joint_limits: + ignored_joint_limit_joint = get_unique_list( [ - self._controlled_joints_dict["arm"], - self._controlled_joints_dict["hand"], + missing_joint_limit.replace("_min", "").replace("_max", "") + for missing_joint_limit in missing_joint_limits ] ) + singular = len(ignored_joint_limit_joint) == 1 + joint_or_joints = "joint" if singular else "joints" + ignored_joint_limit_joint_str = ( + f"'{ignored_joint_limit_joint[0]}'" + if singular + else ", ".join(ignored_joint_limit_joint) + ) + rospy.logwarn( + f"Joint limits specified on {joint_or_joints} " + f"{ignored_joint_limit_joint_str} were ignored as both a min and max " + "limit need to be specified." + ) + joint_limits_names = [ + name + for name in joint_limits_names + if name.replace("_min", "").replace("_max", "") + not in ignored_joint_limit_joint + ] + joint_limits_values = [ + joint_limit + for joint_limit_name, joint_limit in zip( + joint_limits_names, + joint_limits_values, + ) + if joint_limit_name.replace("_min", "").replace("_max", "") + not in ignored_joint_limit_joint + ] - # Get the current state of the arm and hand. - arm_state_dict = dict( - zip( - self.move_group_arm.get_active_joints(), - self.move_group_arm.get_current_joint_values(), + return { + name: value + for name, value in zip( + joint_limits_names, + joint_limits_values, ) - ) - if control_group in ["hand", "both"]: - hand_state_dict = dict( + } + + def _get_random_joint_values(self, group): + """Returns a dictionary containing random joint values for a given control + group. + + Args: + group (str): The control group for which you want to retrieve random joint + values. Options are ``arm`` or ``hand``. + + Returns: + dict: Dictionary containing the random joint values. + """ + group = group.lower() + if group not in ["arm", "hand"]: + raise ValueError( + f"Invalid group '{group}'. Valid values are 'arm' or 'hand'." + ) + + # Retrieve random joint values. + try: + return dict( zip( - self.move_group_hand.get_active_joints(), - self.move_group_hand.get_current_joint_values(), + self._controlled_joints_dict[group], + getattr(self, f"move_group_{group}").get_random_joint_values(), ) ) + except MoveItCommanderException: + return None - # Generate 'moveit_commander' control command - controlled_joints_size = len(controlled_joints) - if len(joint_names) == 0: - # Check if enough joint position commands were given. - if len(joint_positions) > controlled_joints_size: - logwarn_msg = ( - "You specified %s while the Panda Robot %s only %s %s. Only the " - "first %s are used in the control." - % ( - "%s %s" - % ( - len(joint_positions), - "joint position" - if len(joint_positions) == 1 - else "joint positions", - ), - "arm and hand" if control_group == "both" else control_group, - "contain" if control_group == "both" else "contains", - "%s %s" - % ( - controlled_joints_size, - "active joint" - if controlled_joints_size == 1 - else "active joints", - ), - "%s %s" - % ( - controlled_joints_size, - "joint position" - if controlled_joints_size == 1 - else "joint positions", - ), - ) - ) - rospy.logwarn(logwarn_msg) - - # Remove the extra joint commands. - joint_positions = joint_positions[:controlled_joints_size] - elif len(joint_positions) < controlled_joints_size: - logwarn_msg = ( - "You specified %s while the Panda Robot %s %s %s. As a result " - "only the first %s joints are controlled while for the other " - "joints the current robot state is used." - % ( - "%s %s" - % ( - len(joint_positions), - "joint position" - if len(joint_positions) == 1 - else "joint positions", - ), - "arm and hand" if control_group == "both" else control_group, - "contain" if control_group == "both" else "contains", - "%s %s" - % ( - controlled_joints_size, - "active joint" - if controlled_joints_size == 1 - else "active joints", - ), - len(joint_positions), - ) + def _get_bounded_random_joint_values( # noqa: C901 + self, joint_limits, max_attempts + ): + """Returns a dictionary containing bounded random joint values for given joint + limits. + + Args: + joint_limits (dict): Dictionary containing the joint limits. + max_attempts (int): The maximum number of attempts to sample valid random + joint values. + + Returns: + (tuple): tuple containing: + - **random_arm_joint_values** (:obj:`dict`): Dictionary containing the + (bounded) random arm joint values. + - **random_hand_joint_values** (:obj:`dict`): Dictionary containing the + (bounded) random hand joint values. + """ + # Retrieve unbounded random joint values. + rospy.logdebug("Retrieving unbounded random joint values.") + random_arm_joint_values = self._get_random_joint_values("arm") + random_hand_joint_values = ( + self._get_random_joint_values("hand") if self._load_gripper else None + ) + if not joint_limits: + return random_arm_joint_values, random_hand_joint_values + + # If joint limits were given, retrieve bounded random joint values. + joint_commands_valid = { + "arm": False, + "hand": not self._load_gripper, + } + move_groups = { + "arm": self.move_group_arm, + } + random_joint_values = { + "arm": random_arm_joint_values, + } + if self._load_gripper: + move_groups["hand"] = self.move_group_hand + random_joint_values["hand"] = random_hand_joint_values + unique_joints = get_unique_list( + [ + joint_limit_names.replace("_min", "").replace("_max", "") + for joint_limit_names in joint_limits.keys() + ] + ) + for attempt in range(max_attempts): + # Retrieve valid random joint values. + rospy.logdebug( + f"Retrieving valid random joint values. Attempt {attempt + 1} of " + f"{max_attempts}." + ) + for joint in unique_joints: + limb = ( + "hand" if joint in self._controlled_joints_dict["hand"] else "arm" ) - rospy.logwarn(logwarn_msg) - - # Fill the missing joint commands. - if control_group == "both": - joint_states = ( - list(arm_state_dict.values()) + list(hand_state_dict.values()) - if self._arm_states_mask[0] - else list(hand_state_dict.values()) - + list(arm_state_dict.values()) + if joint_commands_valid[limb] or not random_joint_values[limb]: + continue + if joint_limits[joint + "_min"] != joint_limits[joint + "_max"]: + random_joint_values[limb][joint] = np.random.uniform( + joint_limits[joint + "_min"], + joint_limits[joint + "_max"], ) else: - joint_states = ( - list(arm_state_dict.values()) - if control_group == "arm" - else list(hand_state_dict.values()) - ) - joint_positions = ( - joint_positions - + joint_states[len(joint_positions) :] # noqa: E203, E501 - ) + joint_commands_valid[limb] = True - # Return moveit_commander_control command dictionary. - if control_group == "arm": - control_commands = {"arm": joint_positions} - elif control_group == "hand": - control_commands = {"hand": joint_positions} - else: - control_commands = { - "arm": list(compress(joint_positions, self._arm_states_mask)), - "hand": list(compress(joint_positions, self._hand_states_mask)), - } - return control_commands - else: - # Check if enough control values were given. - if len(joint_names) != len(joint_positions): - logwarn_msg = ( - "You specified %s while the 'joint_names' field of the " - "'%s' message contains %s. Please make sure you supply " - "a joint position for each joint contained in the 'joint_names' " - "field." - % ( - "%s %s" - % ( - len(joint_positions), - "joint position" - if len(joint_positions) == 1 - else "joint positions", - ), - "panda_gazebo/SetJointPositions", - "%s %s" - % ( - len(joint_names), - "joint" if len(joint_names) == 1 else "joints", - ), + # Check if joint values are valid. + rospy.logdebug("Checking if joint values are valid.") + for limb in ["arm", "hand"]: + if joint_commands_valid[limb] or not random_joint_values[limb]: + continue + try: + plan = move_groups[limb].plan( + joint_state_dict_2_joint_state_msg(random_joint_values[limb]) ) - ) - rospy.logerr(logwarn_msg) - raise InputMessageInvalidError( - message=( - "Joint_names and joint_positions fields of the input " - "message are of different lengths." - ), - log_message=logwarn_msg, - details={ - "joint_positions_command_length": len(joint_positions), - "joint_names_length": len(joint_names), - }, - ) - - # Validate joint_names. - invalid_joint_names = [ - joint_name - for joint_name in joint_names - if joint_name not in controlled_joints - ] - input_command_dict = dict(zip(joint_names, joint_positions)) - if len(invalid_joint_names) != 0: - # Remove invalid keys and throw warning. - [input_command_dict.pop(joint, None) for joint in invalid_joint_names] - logwarn_msg = ( - "Ignoring %s %s since %s invalid. Valid joint names for " - "controlling the panda %s are %s." - % ( - invalid_joint_names, - "joint" if len(invalid_joint_names) == 1 else "joints", - "it is" if len(invalid_joint_names) == 1 else "they are", - "arm and hand" if control_group == "both" else control_group, - controlled_joints, + joint_commands_valid[limb] = ( + len(plan[1].joint_trajectory.points) != 0 ) + except MoveItCommanderException: + joint_commands_valid[limb] = False + if all(joint_commands_valid.values()): + break + elif attempt == max_attempts - 1: + rospy.logwarn( + "Failed to sample valid random joint positions within the maximum " + f"number of attempts ({max_attempts})." ) - rospy.logwarn(logwarn_msg) - - # Update current state dictionary with given joint_position setpoints. - arm_output_command_dict = copy.deepcopy(arm_state_dict) - for joint, position in input_command_dict.items(): # Update arm. - if joint in arm_state_dict: - arm_output_command_dict[joint] = position - if control_group in ["hand", "both"]: - hand_output_command_dict = copy.deepcopy(hand_state_dict) - for joint, position in input_command_dict.items(): # Update hand. - if joint in hand_state_dict: - hand_output_command_dict[joint] = position - - # Return moveit_commander_control command dictionary. - if control_group == "arm": - control_commands = {"arm": list(arm_output_command_dict.values())} - elif control_group == "hand": - control_commands = {"hand": list(hand_output_command_dict.values())} - else: - control_commands = { - "arm": list(arm_output_command_dict.values()), - "hand": list(hand_output_command_dict.values()), - } - return control_commands + random_arm_joint_values, random_hand_joint_values = None, None + break + + rospy.logwarn( + "Failed to sample valid random joint positions from the bounding " + "region. Trying again." + ) + + return random_arm_joint_values, random_hand_joint_values ############################################### # Service callback functions ################## @@ -866,19 +1014,13 @@ def _arm_get_ee_pose_joint_config(self, get_ee_pose_joint_configuration): get_ee_pose_joint_configuration.pose.orientation ) - pose_target = Pose() - pose_target.orientation.x = get_ee_pose_joint_configuration.pose.orientation.x - pose_target.orientation.y = get_ee_pose_joint_configuration.pose.orientation.y - pose_target.orientation.z = get_ee_pose_joint_configuration.pose.orientation.z - pose_target.orientation.w = get_ee_pose_joint_configuration.pose.orientation.w - pose_target.position.x = get_ee_pose_joint_configuration.pose.position.x - pose_target.position.y = get_ee_pose_joint_configuration.pose.position.y - pose_target.position.z = get_ee_pose_joint_configuration.pose.position.z - # Retrieve joint configurations. + pose_target = get_ee_pose_joint_configuration.pose resp = GetEePoseJointConfigResponse() n_sample = 0 - while True: # Continue till joint positions are valid or max samples size. + while ( + n_sample < max_attempts + ): # Continue till joint positions are valid or max samples size. rospy.logdebug("Retrieving joint configuration for given EE pose.") try: retval, plan, _, _ = self.move_group_arm.plan(pose_target) @@ -887,26 +1029,30 @@ def _arm_get_ee_pose_joint_config(self, get_ee_pose_joint_configuration): # Check if joint config was retrieved. if retval: - resp.joint_names = list(plan.joint_trajectory.joint_names) + resp.joint_names = plan.joint_trajectory.joint_names resp.joint_positions = list(plan.joint_trajectory.points[-1].positions) resp.success = True resp.message = "Everything went OK" break - elif n_sample >= max_attempts: - resp.success = False - resp.message = "Joint configuration could not be retrieved for Ee pose" - break else: rospy.logwarn( - "Failed to retrieve joint configuration for given EE pose Trying " + "Failed to retrieve joint configuration for given EE pose. Trying " f"again ({n_sample})." ) n_sample += 1 + + # Return response. + if n_sample >= max_attempts: + rospy.logwarn( + "Joint configuration could not be retrieved for given EE pose within " + f"the maximum number of attempts ({max_attempts})." + ) + resp.success = False + resp.message = "Joint configuration could not be retrieved for Ee pose" return resp def _arm_set_ee_pose_callback(self, set_ee_pose_req): - """Request the Panda arm to control to a given end effector - (EE) pose. + """Request the Panda arm to control to a given end effector (EE) pose. Args: set_ee_pose_req :obj:`geometry_msgs.msg.Pose`: The trajectory you want the @@ -929,13 +1075,7 @@ def _arm_set_ee_pose_callback(self, set_ee_pose_req): # Fill trajectory message. rospy.logdebug("Setting ee pose.") resp = SetEePoseResponse() - self.ee_pose_target.orientation.x = set_ee_pose_req.pose.orientation.x - self.ee_pose_target.orientation.y = set_ee_pose_req.pose.orientation.y - self.ee_pose_target.orientation.z = set_ee_pose_req.pose.orientation.z - self.ee_pose_target.orientation.w = set_ee_pose_req.pose.orientation.w - self.ee_pose_target.position.x = set_ee_pose_req.pose.position.x - self.ee_pose_target.position.y = set_ee_pose_req.pose.position.y - self.ee_pose_target.position.z = set_ee_pose_req.pose.position.z + self.ee_pose_target = set_ee_pose_req.pose # Send trajectory message and return response. try: @@ -957,7 +1097,8 @@ def _arm_set_ee_pose_callback(self, set_ee_pose_req): return resp def _set_joint_positions_callback(self, set_joint_positions_req): # noqa: C901 - """Request the Panda arm and hand to go to a given joint angle. + """Request the Panda arm and hand to go to a given joint angle and gripper + position. Args: set_joint_positions_req (:obj:`panda_gazebo.srv.SetJointPositionRequest`): @@ -965,29 +1106,15 @@ def _set_joint_positions_callback(self, set_joint_positions_req): # noqa: C901 Returns: :obj:`panda_gazebo.srv.SetJointPositionResponse`: Response message - containing (success bool, message). + containing (success bool, message). """ rospy.logdebug("Setting joint position targets.") - # Check if set_joint_positions_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(set_joint_positions_req.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "positions." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/set_joint_positions" % rospy.get_name(), - ) - ) - # Create moveit_commander command. resp = SetJointPositionsResponse() try: - moveit_commander_commands = self._create_joint_positions_commands( - set_joint_positions_req + moveit_commander_commands = self._create_positions_moveit_setpoints( + set_joint_positions_req, control_group="both" ) except InputMessageInvalidError as e: logwarn_msg = "Panda robot joint positions not set as " + lower_first_char( @@ -999,64 +1126,31 @@ def _set_joint_positions_callback(self, set_joint_positions_req): # noqa: C901 return resp # Set joint positions setpoint. - arm_joint_states = self.move_group_arm.get_current_joint_values() - rospy.logdebug(f"Current arm joint positions: {arm_joint_states}") - if self._load_gripper: - hand_joint_states = self.move_group_hand.get_current_joint_values() - rospy.logdebug(f"Current hand joint positions: {hand_joint_states}") self.joint_positions_target = moveit_commander_commands - set_joint_value_target_success_bool = [] - set_joint_value_target_error_msg = [] - try: - rospy.logdebug( - "Arm joint positions setpoint: %s" % moveit_commander_commands["arm"] + success, failed_groups = True, [] + for group in ["arm", "hand"]: + if group == "hand" and not self._load_gripper: + continue + group_success, group_error_msg = self._set_joint_value_target( + group, moveit_commander_commands[group] ) - self.move_group_arm.set_joint_value_target(moveit_commander_commands["arm"]) - set_joint_value_target_success_bool.append(True) - except MoveItCommanderException as e: - set_joint_value_target_success_bool.append(False) - set_joint_value_target_error_msg.append(e.args[0]) - if self._load_gripper: - try: - rospy.logdebug( - "Hand joint positions setpoint: %s" - % moveit_commander_commands["hand"] - ) - self.move_group_hand.set_joint_value_target( - moveit_commander_commands["hand"] + if not group_success: + success = False + failed_groups.append(group) + rospy.logwarn( + f"Setting {group} joint position targets failed since there was an " + f"{lower_first_char(group_error_msg)}" ) - set_joint_value_target_success_bool.append(True) - except MoveItCommanderException as e: - set_joint_value_target_success_bool.append(False) - set_joint_value_target_error_msg.append(e.args[0]) # Print error message if an error occurred and return. - if set_joint_value_target_error_msg: - if len(set_joint_value_target_error_msg) > 1: - log_warn_string = "arm and hand" - rospy.logwarn( - "Setting arm joint position targets failed since there was an %s" - % (lower_first_char(set_joint_value_target_error_msg[0])) - ) - rospy.logwarn( - "Setting hand joint position targets failed since there was an %s" - % (lower_first_char(set_joint_value_target_error_msg[1])) - ) - else: - log_warn_string = ( - "arm" - if not self._load_gripper - else ("arm" if set_joint_value_target_success_bool[0] else "hand") - ) - rospy.logwarn( - "Setting %s joint position targets failed since there was an %s" - % ( - log_warn_string, - lower_first_char(set_joint_value_target_error_msg[0]), - ) - ) + if not success: + failed_groups_str = ( + " and ".join(failed_groups) + if len(failed_groups) > 1 + else failed_groups[0] + ) resp.success = False - resp.message = f"Failed to set {log_warn_string} setpoints." + resp.message = "Failed to set joint positions for %s." % failed_groups_str return resp # Execute setpoints. @@ -1065,7 +1159,7 @@ def _set_joint_positions_callback(self, set_joint_positions_req): # noqa: C901 retval = self._execute() if not all(retval): resp.success = False - resp.message = "Joint position setpoint could not be set" + resp.message = "Joint position setpoint could not executed" else: resp.success = True resp.message = "Everything went OK" @@ -1080,7 +1174,7 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req): Args: set_joint_positions_req (:obj:`panda_gazebo.srv.SetJointPositionRequest`): - The joint positions you want to control the joints to. + The joint positions request message. Returns: :obj:`panda_gazebo.srv.SetJointPositionResponse`: Response message @@ -1088,26 +1182,12 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req): """ rospy.logdebug("Setting arm joint position targets.") - # Check if set_joint_positions_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(set_joint_positions_req.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "positions." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/panda_arm/set_joint_positions" % rospy.get_name(), - ) - ) - # Create moveit_commander command. resp = SetJointPositionsResponse() try: - moveit_commander_commands = self._create_joint_positions_commands( + moveit_commander_arm_commands = self._create_positions_moveit_setpoints( set_joint_positions_req, control_group="arm" - ) + )["arm"] except InputMessageInvalidError as e: logwarn_msg = "Arm joint Positions not set as " + lower_first_char( e.log_message @@ -1120,12 +1200,12 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req): # Set joint positions setpoint. arm_joint_states = self.move_group_arm.get_current_joint_values() rospy.logdebug(f"Current arm joint positions: {arm_joint_states}") - self.joint_positions_target = moveit_commander_commands + self.joint_positions_target = moveit_commander_arm_commands try: rospy.logdebug( - "Arm joint positions setpoint: %s" % moveit_commander_commands["arm"] + "Arm joint positions setpoint: %s" % moveit_commander_arm_commands ) - self.move_group_arm.set_joint_value_target(moveit_commander_commands["arm"]) + self.move_group_arm.set_joint_value_target(moveit_commander_arm_commands) except MoveItCommanderException as e: rospy.logwarn( "Setting arm joint position targets failed since there was an %s" @@ -1141,7 +1221,7 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req): retval = self._execute(control_group="arm") if not all(retval): resp.success = False - resp.message = "Arm joint position setpoint could not be set" + resp.message = "Arm joint position setpoint could not executed" else: resp.success = True resp.message = "Everything went OK" @@ -1152,11 +1232,11 @@ def _arm_set_joint_positions_callback(self, set_joint_positions_req): return resp def _hand_set_joint_positions_callback(self, set_joint_positions_req): - """Request the Panda arm to go to a given joint angle. + """Set panda hand to a given joint position or gripper width. Args: set_joint_positions_req (:obj:`panda_gazebo.srv.SetJointPositionRequest`): - The joint positions you want to control the joints to. + The joint positions request message. Returns: :obj:`panda_gazebo.srv.SetJointPositionResponse`: Response message @@ -1164,26 +1244,12 @@ def _hand_set_joint_positions_callback(self, set_joint_positions_req): """ rospy.logdebug("Setting hand joint position targets.") - # Check if set_joint_positions_req.joint_names contains duplicates - duplicate_list = get_duplicate_list(set_joint_positions_req.joint_names) - if duplicate_list: - rospy.logwarn( - "Multiple entries were found for %s '%s' in the '%s' message. " - "Consequently, only the last occurrence was used in setting the joint " - "positions." - % ( - "joints" if len(duplicate_list) > 1 else "joint", - duplicate_list, - "%s/panda_hand/set_joint_positions" % rospy.get_name(), - ) - ) - # Create moveit_commander command. resp = SetJointPositionsResponse() try: - moveit_commander_commands = self._create_joint_positions_commands( + hand_moveit_set_point = self._create_positions_moveit_setpoints( set_joint_positions_req, control_group="hand" - ) + )["hand"] except InputMessageInvalidError as e: logwarn_msg = "Hand joint Positions not set as " + lower_first_char( e.log_message @@ -1196,14 +1262,10 @@ def _hand_set_joint_positions_callback(self, set_joint_positions_req): # Set joint positions setpoint. hand_joint_states = self.move_group_hand.get_current_joint_values() rospy.logdebug(f"Current hand joint positions: {hand_joint_states}") - self.joint_positions_target = moveit_commander_commands + self.joint_positions_target = hand_moveit_set_point try: - rospy.logdebug( - "Hand joint positions setpoint: %s" % moveit_commander_commands["hand"] - ) - self.move_group_hand.set_joint_value_target( - moveit_commander_commands["hand"] - ) + rospy.logdebug("Hand joint positions setpoint: %s" % hand_moveit_set_point) + self.move_group_hand.set_joint_value_target(hand_moveit_set_point) except MoveItCommanderException as e: rospy.logwarn( "Setting hand joint position targets failed since there was an %s" @@ -1219,7 +1281,7 @@ def _hand_set_joint_positions_callback(self, set_joint_positions_req): retval = self._execute(control_group="hand") if not all(retval): resp.success = False - resp.message = "Hand joint position setpoint could not be set" + resp.message = "Hand joint position setpoint could not be executed" else: resp.success = True resp.message = "Everything went OK" @@ -1329,345 +1391,84 @@ def _get_random_joint_positions_callback( # noqa: C901 Please be aware that when the ``min`` and ``max`` boundaries of a joint are set to be equal the joint is assumed to be unbounded. """ + resp = GetRandomJointPositionsResponse( + success=True, message="Everything went OK" + ) max_attempts = ( get_random_position_req.attempts if get_random_position_req.attempts != 0.0 else MAX_RANDOM_SAMPLES ) - joint_limits = {} - limited_joints = [] - arm_joints = self.move_group_arm.get_active_joints() - hand_joints = ( - self.move_group_hand.get_active_joints() if self._load_gripper else [] - ) - - # Validate joint limits if supplied (remove them if invalid) - if ( - get_random_position_req.joint_limits.names - and get_random_position_req.joint_limits.values - ): - get_random_position_req.joint_limits.names = [ - joint_limit_name.lower() - for joint_limit_name in get_random_position_req.joint_limits.names - ] - - # Check if limit names and limit values are of equal length. - if len(get_random_position_req.joint_limits.names) != len( - get_random_position_req.joint_limits.values - ): - rospy.logwarn( - "Joint limits ignored as the number of joints ({}) is " - "unequal to the number of limit values ({}).".format( - len(get_random_position_req.joint_limits.names), - len(get_random_position_req.joint_limits.values), - ) - ) - get_random_position_req.joint_limits.names = [] - get_random_position_req.joint_limits.values = [] - else: # Check if the names in the joint_limits message are valid. - valid_joint_names = flatten_list( - [ - [joint + "_min", joint + "_max"] - for joint in flatten_list([arm_joints, hand_joints]) - ] - ) - invalid_joint_names = [] - for name in get_random_position_req.joint_limits.names: - if name not in valid_joint_names: - invalid_joint_names.append(name) - - # Throw warning and remove invalid joint limits. - if len(invalid_joint_names) != 0: - warn_strings = ( - [ - "limit", - invalid_joint_names[0], - "was", - "it is not a valid joint limit", - ] - if len(invalid_joint_names) == 1 - else [ - "limits", - invalid_joint_names, - "were", - "they are not valid joint limits", - ] - ) - rospy.logwarn( - "Joint {} '{}' {} ignored since {}. Valid values are " - "'{}'.".format( - warn_strings[0], - warn_strings[1], - warn_strings[2], - warn_strings[3], - valid_joint_names, - ) - ) - get_random_position_req.joint_limits.names = [ - joint_name - for joint_name in get_random_position_req.joint_limits.names - if joint_name not in invalid_joint_names - ] - get_random_position_req.joint_limits.values = [ - joint_limit - for joint_limit_name, joint_limit in zip( - get_random_position_req.joint_limits.names, - get_random_position_req.joint_limits.values, - ) - if joint_limit_name not in invalid_joint_names - ] - - # Check if a joint limit both has a min and max specified. - limited_joints = get_unique_list( - [ - name.replace("_min", "").replace("_max", "") - for name in get_random_position_req.joint_limits.names - ] - ) - required_joint_limits = flatten_list( - [[joint + "_min", joint + "_max"] for joint in limited_joints] - ) - missing_joint_limits = [ - required_joint_limit - for required_joint_limit in required_joint_limits - if required_joint_limit - not in get_random_position_req.joint_limits.names - ] - if missing_joint_limits: - ignored_joint_limit_joint = get_unique_list( - [ - missing_joint_limit.replace("_min", "").replace("_max", "") - for missing_joint_limit in missing_joint_limits - ] - ) - warn_strings = ( - ["joint", f"'{ignored_joint_limit_joint}'"] - if len(ignored_joint_limit_joint) == 1 - else ["joints", ignored_joint_limit_joint] - ) - rospy.logwarn( - "Joint limits specified on {} {} were ignored as both a min " - "and max limit need to be specified.".format(*warn_strings) - ) - get_random_position_req.joint_limits.names = [ - name - for name in get_random_position_req.joint_limits.names - if name.replace("_min", "").replace("_max", "") - not in ignored_joint_limit_joint - ] - get_random_position_req.joint_limits.values = [ - joint_limit - for joint_limit_name, joint_limit in zip( - get_random_position_req.joint_limits.names, - get_random_position_req.joint_limits.values, - ) - if joint_limit_name.replace("_min", "").replace("_max", "") - not in ignored_joint_limit_joint - ] - joint_limits = dict( - zip( - get_random_position_req.joint_limits.names, - get_random_position_req.joint_limits.values, - ) - ) - # Retrieve random joint position values. - get_random_arm_joint_positions_srvs_exception = False - get_random_hand_joint_positions_srvs_exception = False - try: - random_arm_joint_values_unbounded = dict( - zip(arm_joints, self.move_group_arm.get_random_joint_values()) + # Retrieve valid joint limits. + joint_limits_values = get_random_position_req.joint_limits.values + joint_limits_names = [ + joint_name.lower() + for joint_name in get_random_position_req.joint_limits.names + ] + if joint_limits_names and joint_limits_values: + joint_limits = self._get_valid_joint_limits( + joint_limits_names, get_random_position_req.joint_limits.values ) - except MoveItCommanderException: - get_random_arm_joint_positions_srvs_exception = True - if self._load_gripper: - try: - random_hand_joint_values_unbounded = dict( - zip(hand_joints, self.move_group_hand.get_random_joint_values()) - ) - except MoveItCommanderException: - get_random_hand_joint_positions_srvs_exception = True - - # Get random joint positions (while taking into possible joint limits) - resp = GetRandomJointPositionsResponse() - random_arm_joint_values = random_arm_joint_values_unbounded.copy() - random_hand_joint_values = ( - random_hand_joint_values_unbounded.copy() if self._load_gripper else {} - ) - if not joint_limits: # If no joint limits were given. - if ( - not get_random_arm_joint_positions_srvs_exception - and not get_random_hand_joint_positions_srvs_exception - ): - resp.success = True - resp.message = "Everything went OK" - else: - resp.success = False - resp.message = "Random joint position could not be retrieved." - else: # Joint limits were set. - # Try to find random joint values within the joint limits. - n_sample = 0 - arm_joint_commands_valid = False - hand_joint_commands_valid = False if self._load_gripper else True - while True: # Continue till joint positions are valid or max samples size. - rospy.logdebug("Retrieving valid random joint positions.") - for joint in get_unique_list( - [ - joint_limit_names.replace("_min", "").replace("_max", "") - for joint_limit_names in joint_limits.keys() - ] - ): - # Sample random value for the given joint within the joint limits. - if ( - joint in random_arm_joint_values.keys() - and not arm_joint_commands_valid - ): - if ( - joint_limits[joint + "_min"] != joint_limits[joint + "_max"] - ): # Only sample if min and max are unequal. - random_arm_joint_values[joint] = np.random.uniform( - joint_limits[joint + "_min"], - joint_limits[joint + "_max"], - ) - else: - arm_joint_commands_valid = True - if ( - joint in random_hand_joint_values.keys() - and not hand_joint_commands_valid - ): - if joint_limits[joint + "_min"] != joint_limits[joint + "_max"]: - random_hand_joint_values[joint] = np.random.uniform( - joint_limits[joint + "_min"], - joint_limits[joint + "_max"], - ) - else: - hand_joint_commands_valid = True - - # Check if joint positions are valid (Plan is not empty) - if not arm_joint_commands_valid: - try: - arm_plan = self.move_group_arm.plan( - joint_state_dict_2_joint_state_msg(random_arm_joint_values) - ) - if len(arm_plan[1].joint_trajectory.points) != 0: - arm_joint_commands_valid = True - else: - arm_joint_commands_valid = False - except MoveItCommanderException: - arm_joint_commands_valid = False - if not hand_joint_commands_valid: - try: - hand_plan = self.move_group_hand.plan( - joint_state_dict_2_joint_state_msg(random_hand_joint_values) - ) - if len(hand_plan[1].joint_trajectory.points) != 0: - hand_joint_commands_valid = True - else: - hand_joint_commands_valid = False - except MoveItCommanderException: - hand_joint_commands_valid = False - - # Set success boolean and break out of loop if joint positions are valid - # otherwise keep sampling till the max sample limit has been reached. - if arm_joint_commands_valid and hand_joint_commands_valid: # If valid. - resp.success = True - resp.message = "Everything went OK" - break - elif n_sample >= max_attempts: - if ( - not get_random_arm_joint_positions_srvs_exception - and not get_random_hand_joint_positions_srvs_exception - ): - warn_string = ( - "hand and arm" - if not ( - hand_joint_commands_valid and arm_joint_commands_valid - ) - else ("hand" if hand_joint_commands_valid else "arm") - ) - rospy.logwarn( - ( - "No valid random {} positions could be found within " - "the set boundary region within the set number of " - "attempts ({}). Please make sure that the robot joints " - "can reach the set joint_limits. Unbounded random " - "{} joint positions returned instead." - ).format(warn_string, max_attempts, warn_string) - ) - resp.success = False - resp.message = ( - "Random {} joint positions could not be retrieved in the " - "boundary region within the set attempts. Unbounded {} " - "random joint positions returned instead." - ).format(warn_string, warn_string) - random_arm_joint_values = random_arm_joint_values_unbounded - if self._load_gripper: - random_hand_joint_values = ( - random_hand_joint_values_unbounded - ) - else: - resp.success = False - resp.message = "Random joint position could not be retrieved." - break - else: - rospy.logwarn( - "Failed to sample valid random joint positions from the " - "bounding region. Trying again." - ) - n_sample += 1 # Increase sampling counter. - - # Fill and resturn response message. - resp.joint_names = ( - flatten_list( - [ - list(random_arm_joint_values.keys()), - list(random_hand_joint_values.keys()), - ] + else: + rospy.logwarn( + "Joint limits ignored as either the joint names or the joint values " + "were not specified." ) - if self._arm_states_mask[0] - else flatten_list( - [ - list(random_hand_joint_values.keys()), - list(random_arm_joint_values.keys()), - ] + joint_limits = {} + + # Retrieve random joint values. + ( + random_arm_joint_values, + random_hand_joint_values, + ) = self._get_bounded_random_joint_values(joint_limits, max_attempts) + + # Return failure if no random joint positions could be retrieved. + if random_arm_joint_values is None or random_hand_joint_values is None: + warn_string = ( + "hand and arm" + if random_arm_joint_values is None and random_hand_joint_values is None + else ("hand" if random_hand_joint_values is None else "arm"), + "with the set joint limits" if joint_limits else ".", ) - ) - resp.joint_positions = ( - flatten_list( - [ - list(random_arm_joint_values.values()), - list(random_hand_joint_values.values()), - ] + resp.success = False + resp.message = ( + f"Random {warn_string[0]} joint positions could not be retrieved " + f"{warn_string[1]}" ) + return resp + + # Fill and return successful response. + joint_order = ( + [random_arm_joint_values, random_hand_joint_values] if self._arm_states_mask[0] - else flatten_list( - [ - list(random_hand_joint_values.values()), - list(random_arm_joint_values.values()), - ] - ) + else [random_hand_joint_values, random_arm_joint_values] + ) + resp.joint_names = flatten_list( + [list(joint_values.keys()) for joint_values in joint_order] + ) + resp.joint_positions = flatten_list( + [list(joint_values.values()) for joint_values in joint_order] ) return resp - def _get_random_ee_pose_callback(self, get_random_ee_pose_req): # noqa: C901 - """Returns valid ee pose for the Panda arm. This function also makes sure that - the ee pose is within a bounding region, if one is supplied. + def _get_random_ee_pose_callback(self, get_random_ee_pose_req): + """Returns a valid random end effector (EE) pose for the Panda arm, along with + the joint positions corresponding to this EE pose. This function ensures that + the EE pose is within a specified bounding region, if one is provided. Args: get_random_ee_pose_req :obj:`std_srvs.srv.Empty`: Empty request. Returns: :obj:`panda_gazebo.srv.GetRandomEePoseResponse`: Response message containing - the random joints positions. - - .. note:: - Additionally also returns the joint_positions that relate to the random EE - pose. + the random ee pose and accompanying joint positions. .. important:: - Please be aware that when the ``min`` and ``max`` boundary of a EE coordinate - are equal the dimension is assumed to be be unbounded. + Please be aware that when the ``min`` and ``max`` boundary of a EE + coordinate are equal the dimension is assumed to be be unbounded. """ + resp = GetRandomEePoseResponse() max_attempts = ( get_random_ee_pose_req.attempts if get_random_ee_pose_req.attempts != 0.0 @@ -1675,129 +1476,139 @@ def _get_random_ee_pose_callback(self, get_random_ee_pose_req): # noqa: C901 ) # Get a random ee pose. + rospy.logdebug("Retrieving a valid random end effector pose.") try: random_ee_pose_unbounded = self.move_group_arm.get_random_pose() - get_random_pose_srvs_exception = False - except MoveItCommanderException: - get_random_pose_srvs_exception = True + except MoveItCommanderException as e: + rospy.logwarn("No random ee pose could be retrieved: %s" % e.args[0]) + resp.success = False + resp.message = "Random ee pose could not be retrieved." + return resp - # Return random ee pose (while taking into account a possible bounding region) - resp = GetRandomEePoseResponse() + # Retrieve corresponding joint positions. + retval, plan, _, _ = self.move_group_arm.plan(random_ee_pose_unbounded) + if not retval: + rospy.logwarn( + "Corresponding joint configuration could not be retrieved for the" + "random ee pose." + ) + resp.success = False + resp.message = ( + "Joint configuration could not be retrieved for random ee pose" + ) + return resp + + # Return response if no bounding region is set. + bounding_region = get_random_ee_pose_req.bounding_region if ( sum( [ - get_random_ee_pose_req.bounding_region.x_max - - get_random_ee_pose_req.bounding_region.x_min, - get_random_ee_pose_req.bounding_region.y_max - - get_random_ee_pose_req.bounding_region.y_min, - get_random_ee_pose_req.bounding_region.z_max - - get_random_ee_pose_req.bounding_region.z_min, + bounding_region.x_max - bounding_region.x_min, + bounding_region.y_max - bounding_region.y_min, + bounding_region.z_max - bounding_region.z_min, ] ) == 0.0 - ): # No bounding region was set. - if not get_random_pose_srvs_exception: + ): + resp.success = True + resp.message = "Everything went OK" + resp.ee_pose = random_ee_pose_unbounded.pose + resp.joint_names = plan.joint_trajectory.joint_names + resp.joint_positions = list(plan.joint_trajectory.points[-1].positions) + return resp + x_bound_set = bounding_region.x_min != bounding_region.x_max + y_bound_set = bounding_region.y_min != bounding_region.y_max + z_bound_set = bounding_region.z_min != bounding_region.z_max + + # Return response if ee pose is within the bounding region. + if ( + ( + not x_bound_set + or bounding_region.x_min + <= random_ee_pose_unbounded.pose.position.x + <= bounding_region.x_max + ) + and ( + not y_bound_set + or bounding_region.y_min + <= random_ee_pose_unbounded.pose.position.y + <= bounding_region.y_max + ) + and ( + not z_bound_set + or bounding_region.z_min + <= random_ee_pose_unbounded.pose.position.z + <= bounding_region.z_max + ) + ): + resp.success = True + resp.message = "Everything went OK" + resp.ee_pose = random_ee_pose_unbounded.pose + resp.joint_names = plan.joint_trajectory.joint_names + resp.joint_positions = list(plan.joint_trajectory.points[-1].positions) + return resp + + # Try to find a valid ee pose if it is not within the bounding region. + for n_sample in range(max_attempts): + sampled_ee_position = np.random.uniform( + [ + get_random_ee_pose_req.bounding_region.x_min, + get_random_ee_pose_req.bounding_region.y_min, + get_random_ee_pose_req.bounding_region.z_min, + ], + [ + get_random_ee_pose_req.bounding_region.x_max, + get_random_ee_pose_req.bounding_region.y_max, + get_random_ee_pose_req.bounding_region.z_max, + ], + size=3, + ) + + # Check if ee pose is valid. + # Use unbounded ee pose if the bounding region is unbounded (np.inf). + random_ee_pose = Pose() + random_ee_pose.position.x = ( + sampled_ee_position[0] + if x_bound_set + else random_ee_pose_unbounded.pose.position.x + ) + random_ee_pose.position.y = ( + sampled_ee_position[1] + if y_bound_set + else random_ee_pose_unbounded.pose.position.y + ) + random_ee_pose.position.z = ( + sampled_ee_position[2] + if z_bound_set + else random_ee_pose_unbounded.pose.position.z + ) + random_ee_pose.orientation = random_ee_pose_unbounded.pose.orientation + try: + retval, plan, _, _ = self.move_group_arm.plan(random_ee_pose) + except MoveItCommanderException: + retval = False + + # Return response if valid ee pose was found. + if retval: resp.success = True resp.message = "Everything went OK" - resp.ee_pose = random_ee_pose_unbounded.pose - else: - resp.success = False - resp.message = "Random ee pose could not be retrieved." - else: # A bounding region was set. - # Try to find a valid ee_pose within the bounding region. - n_sample = 0 - ee_pose_valid = False - while True: # Continue till ee pose is valid or max samples size. - rospy.logdebug("Retrieving a valid random end effector pose.") - - # Sample ee position from bounding region. - sampled_ee_position = np.random.uniform( - [ - get_random_ee_pose_req.bounding_region.x_min, - get_random_ee_pose_req.bounding_region.y_min, - get_random_ee_pose_req.bounding_region.z_min, - ], - [ - get_random_ee_pose_req.bounding_region.x_max, - get_random_ee_pose_req.bounding_region.y_max, - get_random_ee_pose_req.bounding_region.z_max, - ], - size=3, - ) + resp.ee_pose = random_ee_pose + resp.joint_names = plan.joint_trajectory.joint_names + resp.joint_positions = list(plan.joint_trajectory.points[-1].positions) + return resp - # Create ee_pose msg. - # NOTE: Only use boundary when {x,y,z}_min and {x,y,z}_max are different - random_ee_pose = Pose() - random_ee_pose.position.x = ( - sampled_ee_position[0] - if ( - get_random_ee_pose_req.bounding_region.x_min - != get_random_ee_pose_req.bounding_region.x_max - ) - else random_ee_pose_unbounded.pose.position.x - ) - random_ee_pose.position.y = ( - sampled_ee_position[1] - if ( - get_random_ee_pose_req.bounding_region.y_min - != get_random_ee_pose_req.bounding_region.y_max - ) - else random_ee_pose_unbounded.pose.position.y - ) - random_ee_pose.position.z = ( - sampled_ee_position[2] - if ( - get_random_ee_pose_req.bounding_region.z_min - != get_random_ee_pose_req.bounding_region.z_max - ) - else random_ee_pose_unbounded.pose.position.z - ) - random_ee_pose.orientation = random_ee_pose_unbounded.pose.orientation + rospy.logwarn( + "Random end effector pose not within the set boundary region. " + f"Trying again ({n_sample+1})." + ) - # Check if pose is valid (No exception and plan is not empty) - try: - plan = self.move_group_arm.plan(random_ee_pose) - ee_pose_valid = ( - True if len(plan[1].joint_trajectory.points) != 0 else False - ) - except MoveItCommanderException: - ee_pose_valid = False - - # Fill response message and break out of loop and if ee pose is valid - # otherwise keep sampling till the max sample limit has been reached. - if ee_pose_valid: # If valid. - resp.success = True - resp.message = "Everything went OK" - resp.ee_pose = random_ee_pose - resp.joint_names = plan[1].joint_trajectory.joint_names - resp.joint_positions = plan[1].joint_trajectory.points[-1].positions - break - elif n_sample >= max_attempts: - if not get_random_pose_srvs_exception: - rospy.logwarn( - "No valid random EE pose could be found within the set " - "boundary region within the set number of attempts " - f"({max_attempts}). Please make sure that the robot " - "end-effector can reach the points inside the bounding " - "region. Unbounded random EE pose returned instead." - ) - resp.success = False - resp.message = ( - "Random ee pose could not be retrieved within the boundary " - "within the set attempts. Unbounded random EE pose " - "returned instead." - ) - resp.ee_pose = random_ee_pose_unbounded.pose - else: - resp.success = False - resp.message = "Random ee pose could not be retrieved." - break - else: - rospy.logwarn( - "Failed to sample a valid random end effector pose from the " - f"bounding region. Trying again ({n_sample})." - ) - n_sample += 1 + # Return failure if no valid ee pose was found. + rospy.logwarn( + "No valid random end effector pose could be found within the set " + "boundary region within the set number of attempts (%s). " % max_attempts + ) + resp.success = False + resp.message = "Valid random end effector pose could not be retrieved." return resp def _get_controlled_joints_cb(self, _): @@ -1816,21 +1627,7 @@ def _get_controlled_joints_cb(self, _): resp.success = True resp.message = "Everything went OK" try: - resp.controlled_joints = ( - flatten_list( - [ - self._controlled_joints_dict["hand"], - self._controlled_joints_dict["arm"], - ] - ) - if self._joint_states.name[0] in self._controlled_joints_dict["hand"] - else flatten_list( - [ - self._controlled_joints_dict["arm"], - self._controlled_joints_dict["hand"], - ] - ) - ) + resp.controlled_joints = self._controlled_joints_dict["both"] resp.controlled_joints_arm = self._controlled_joints_dict["arm"] resp.controlled_joints_hand = self._controlled_joints_dict["hand"] except InputMessageInvalidError: @@ -1847,23 +1644,28 @@ def _scene_add_box_callback(self, add_box_req): Returns: bool: Whether the box was successfully added. """ - pose_header = Header( - frame_id=add_box_req.frame_id if add_box_req.frame_id else "world" - ) + box_name = add_box_req.name or "box" + pose_header = Header(frame_id=add_box_req.frame_id or "world") box_pose = Pose( position=add_box_req.pose.position, orientation=normalize_quaternion(add_box_req.pose.orientation), ) + pose_stamped = PoseStamped(header=pose_header, pose=box_pose) + + # Warn users if box is not visible in RViz. + if add_box_req.size == (0.0, 0.0, 0.0): + rospy.logwarn( + f"The size of box '{box_name}' was set to (0, 0, 0). The box will not " + "be visible in RViz." + ) # Send request. resp = AddBoxResponse() try: self.scene.add_box( - add_box_req.name if add_box_req.name else "box", - PoseStamped(header=pose_header, pose=box_pose) - if add_box_req.pose - else PoseStamped(orientation=Quaternion(0, 0, 0, 1)), - size=add_box_req.size if add_box_req.size else (1, 1, 1), + box_name, + pose_stamped, + size=add_box_req.size, ) except Exception: resp.success = False @@ -1885,10 +1687,11 @@ def _scene_add_plane_callback(self, add_plane_req): Returns: bool: Whether the plane was successfully added. """ + plane_name = add_plane_req.name or "plane" pose_header = Header(frame_id=add_plane_req.frame_id or "world") plane_pose = Pose( position=add_plane_req.pose.position, - orientation=add_plane_req.pose.orientation, + orientation=normalize_quaternion(add_plane_req.pose.orientation), ) pose_stamped = PoseStamped(header=pose_header, pose=plane_pose) @@ -1896,7 +1699,7 @@ def _scene_add_plane_callback(self, add_plane_req): resp = AddPlaneResponse() try: self.scene.add_plane( - add_plane_req.name or "plane", + plane_name, pose_stamped, normal=normalize_vector(add_plane_req.normal, force=True), offset=add_plane_req.offset, diff --git a/panda_gazebo/tests/manual/moveit_commander_test.py b/panda_gazebo/tests/manual/moveit_commander_test.py old mode 100644 new mode 100755 index 1991b0b1..0f958834 --- a/panda_gazebo/tests/manual/moveit_commander_test.py +++ b/panda_gazebo/tests/manual/moveit_commander_test.py @@ -3,14 +3,16 @@ import sys import math -from geometry_msgs.msg import Pose +from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point import moveit_commander import moveit_msgs.msg +from std_msgs.msg import Header import rospy if __name__ == "__main__": # Initialise MoveIt commander and node. moveit_commander.roscpp_initialize(sys.argv) + print(sys.argv) rospy.init_node("move_group_python_interface_tutorial", anonymous=True) # Create commanders. @@ -19,7 +21,7 @@ # Load arm and hand groups. move_group = moveit_commander.MoveGroupCommander("panda_arm") - hand_move_group = moveit_commander.MoveGroupCommander("hand") + hand_move_group = moveit_commander.MoveGroupCommander("panda_hand") display_trajectory_publisher = rospy.Publisher( "/move_group/display_planned_path", @@ -45,60 +47,79 @@ print(robot.get_current_state()) print("") - # -- Plan arm joint goal -- - - # We get the joint values from the group and change some of the values: - joint_goal = move_group.get_current_joint_values() - joint_goal[0] = 0 - joint_goal[1] = -math.tau / 8 - joint_goal[2] = 0 - joint_goal[3] = -math.tau / 4 - joint_goal[4] = 0 - joint_goal[5] = math.tau / 6 # 1/6 of a turn - joint_goal[6] = 0 - - # METHOD 1 - # -- UNCOMMENT BELOW TO SEE THE SCALING FACTOR IN ACTION -- - # max_vel_scale_factor = 0.2 - # max_vel_scale_factor = 1.0 - # max_acc_scale_factor = 0.2 - # max_acc_scale_factor = 1.0 - # move_group.set_max_velocity_scaling_factor(max_vel_scale_factor) - # move_group.set_max_acceleration_scaling_factor(max_acc_scale_factor) - # -- UNCOMMENT ABOVE TO SEE THE SCALING FACTOR IN ACTION -- - move_group.set_joint_value_target(joint_goal) - move_group.plan() - move_group.go(wait=True) - - # METHOD 2 - # The go command can be called with joint values, poses, or without any - # parameters if you have already set the pose or joint target for the group. - move_group.go(joint_goal, wait=True) - - # Calling ``stop()`` ensures that there is no residual movement - move_group.stop() - - # -- Plan pose goal -- - pose_goal = Pose() - pose_goal.orientation.w = 1.0 - pose_goal.position.x = 0.4 - pose_goal.position.y = 0.1 - pose_goal.position.z = 0.4 - - move_group.set_pose_target(pose_goal) - plan = move_group.go(wait=True) - - # Calling `stop()` ensures that there is no residual movement - move_group.stop() - - # It is always good to clear your targets after planning with poses. - # Note: there is no equivalent function for clear_joint_value_targets() - move_group.clear_pose_targets() + # # -- Plan arm joint goal -- + + # # We get the joint values from the group and change some of the values: + # joint_goal = move_group.get_current_joint_values() + # joint_goal[0] = 0 + # joint_goal[1] = -math.tau / 8 + # joint_goal[2] = 0 + # joint_goal[3] = -math.tau / 4 + # joint_goal[4] = 0 + # joint_goal[5] = math.tau / 6 # 1/6 of a turn + # joint_goal[6] = 0 + + # # METHOD 1 + # # -- UNCOMMENT BELOW TO SEE THE SCALING FACTOR IN ACTION -- + # # max_vel_scale_factor = 0.2 + # # max_vel_scale_factor = 1.0 + # # max_acc_scale_factor = 0.2 + # # max_acc_scale_factor = 1.0 + # # move_group.set_max_velocity_scaling_factor(max_vel_scale_factor) + # # move_group.set_max_acceleration_scaling_factor(max_acc_scale_factor) + # # -- UNCOMMENT ABOVE TO SEE THE SCALING FACTOR IN ACTION -- + # move_group.set_joint_value_target(joint_goal) + # move_group.plan() + # move_group.go(wait=True) + + # # METHOD 2 + # # The go command can be called with joint values, poses, or without any + # # parameters if you have already set the pose or joint target for the group. + # move_group.go(joint_goal, wait=True) + + # # Calling ``stop()`` ensures that there is no residual movement + # move_group.stop() + + # # # -- Plan pose goal -- + # pose_goal = Pose() + # pose_goal.orientation.w = 1.0 + # pose_goal.position.x = 400 + # pose_goal.position.y = 100 + # pose_goal.position.z = 0.4 + + # # move_group.set_pose_target(pose_goal) + # retval, plan, _, error_code = move_group.plan(pose_goal) + # plan = move_group.go(wait=True) + + # # Calling `stop()` ensures that there is no residual movement + # move_group.stop() + + # # It is always good to clear your targets after planning with poses. + # # Note: there is no equivalent function for clear_joint_value_targets() + # move_group.clear_pose_targets() # -- Plan hand goal -- - hand_move_group.set_joint_value_target([0.04, 0.04]) + # hand_move_group.set_joint_value_target([0.04, 0.04]) + hand_move_group.set_joint_value_target([0.04]) (hand_plan_retval, plan, _, error_code) = hand_move_group.plan() retval = hand_move_group.execute(plan, wait=True) # Calling ``stop()`` ensures that there is no residual movement move_group.stop() + + # # -- Add plane to scene -- + pose_stamped = PoseStamped( + # header=Header(frame_id="panda_link0"), + header=Header(frame_id="world"), + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + ) + scene.add_plane( + "plane", + pose_stamped, + normal=(0.0, 0.0, 1.0), + offset=0.0, + ) + print("Added plane to scene") diff --git a/panda_gazebo/tests/manual/panda_moveit_server_test.py b/panda_gazebo/tests/manual/panda_moveit_server_test.py old mode 100644 new mode 100755 index ace9c2a7..e819c6e9 --- a/panda_gazebo/tests/manual/panda_moveit_server_test.py +++ b/panda_gazebo/tests/manual/panda_moveit_server_test.py @@ -34,40 +34,40 @@ if __name__ == "__main__": rospy.init_node("test_panda_moveit_server") - # # -- Test set robot joint positions service -- - # req = SetJointPositionsRequest() - # req.joint_positions = [ - # 2.15, - # 1.12, - # -1.59, - # -1.94, - # -2.44, - # 1.88, - # 1.54, - # 0.02, - # 0.02, - # ] - # req.joint_names = [ - # "panda_joint1", - # "panda_joint2", - # "panda_joint3", - # "panda_joint4", - # "panda_joint5", - # "panda_joint6", - # "panda_joint7", - # "panda_finger_joint1", - # "panda_finger_joint2", - # ] - # set_joint_positions_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/set_joint_positions", SetJointPositions - # ) - # resp = set_joint_positions_srv.call(req) - # print(resp.message) + # -- Test set robot joint positions service -- + req = SetJointPositionsRequest() + req.joint_positions = [ + 2.15, + 1.12, + -1.59, + -1.94, + -2.44, + 1.88, + 1.54, + 0.02, + # 0.02, + ] + req.joint_names = [ + # "panda_joint1", + # "panda_joint2", + # "panda_joint3", + # "panda_joint4", + # "panda_joint5", + # "panda_joint6", + # "panda_joint7", + # "panda_finger_joint1", + # "panda_finger_joint2", + ] + set_joint_positions_srv = rospy.ServiceProxy( + "panda_moveit_planner_server/set_joint_positions", SetJointPositions + ) + resp = set_joint_positions_srv.call(req) + print(resp.message) # # -- Test panda_arm set robot joint positions service -- # req = SetJointPositionsRequest() # req.joint_positions = [-0.60, 0.33, -0.83, -1.46, 0.26, 1.69, -0.64, 0.016, 0.016] - # # req.joint_names = ["panda_joint1", "panda_joint2"] + # req.joint_names = ["panda_joint1", "panda_joint2"] # setarm__joint_positions_srv = rospy.ServiceProxy( # "panda_moveit_planner_server/panda_arm/set_joint_positions", SetJointPositions # ) @@ -76,9 +76,10 @@ # # -- Test set panda_hand robot joint positions service -- # req = SetJointPositionsRequest() - # req.joint_positions = [0.0, 0.0] - # # req.joint_positions = [0.03] - # # req.joint_names = ["panda_finger_joint1"] + # req.joint_names = ["panda_finger_joint1", "panda_finger_joint2"] + # req.joint_positions = [0.04, 0.0] + # # req.joint_names = ["gripper_width"] + # # req.joint_positions = [0.08] # set_hand_joint_positions_srv = rospy.ServiceProxy( # "panda_moveit_planner_server/panda_hand/set_joint_positions", # SetJointPositions, @@ -89,7 +90,7 @@ # # -- Test set ee pose service -- # req = SetEePoseRequest() # req.pose.position.x = 0 - # req.pose.position.y = 0.5 + # req.pose.position.y = 0.4 # req.pose.position.z = 0.5 # set_ee_pose_srv = rospy.ServiceProxy( # "panda_moveit_planner_server/panda_arm/set_ee_pose", SetEePose @@ -133,9 +134,6 @@ # # Set back. # req = SetEeRequest() # req.ee_name = "panda_link8" - # set_ee_srv = rospy.ServiceProxy( - # "panda_moveit_planner_server/panda_arm/set_ee", SetEe - # ) # resp = set_ee_srv.call(req) # # -- Test get random joint positions service -- @@ -146,7 +144,7 @@ # "panda_finger_joint1_min", # "panda_finger_joint1_max", # ] - # req.joint_limits.values = [0.0, 0.1, 0.0, 0.04] + # req.joint_limits.values = [0.25, 0.5, 0.5, 0.10] # get_random_joint_positions_srv = rospy.ServiceProxy( # "panda_moveit_planner_server/get_random_joint_positions", # GetRandomJointPositions, @@ -154,15 +152,15 @@ # resp = get_random_joint_positions_srv.call(req) # print(resp.message) - # -- Test get random pose service -- - req = GetRandomEePoseRequest() - req.bounding_region = BoundingRegion(x_min=0.0, x_max=0.5) - get_random_ee_pose_srv = rospy.ServiceProxy( - "panda_moveit_planner_server/get_random_ee_pose", - GetRandomEePose, - ) - resp = get_random_ee_pose_srv.call(req) - print(resp.message) + # # -- Test get random pose service -- + # req = GetRandomEePoseRequest() + # req.bounding_region = BoundingRegion(x_min=0.0, x_max=0.05) + # get_random_ee_pose_srv = rospy.ServiceProxy( + # "panda_moveit_planner_server/get_random_ee_pose", + # GetRandomEePose, + # ) + # resp = get_random_ee_pose_srv.call(req) + # print(resp.message) # # -- Test get ee pose joint config service -- # req = GetEePoseJointConfigRequest() @@ -188,9 +186,11 @@ # print(resp.message) # # -- Test add Box service -- - # # req = AddBoxRequest() + # req = AddBoxRequest() # req = AddBoxRequest( - # name="box", pose=Pose(orientation=Quaternion(0, 0, 0, 1)), size=[1, 1, 1] + # # name="box", pose=Pose(orientation=Quaternion(0, 0, 0, 1)), size=[1, 1, 1] + # # name="box", size=[1, 1, 1] + # name="box2", # ) # add_box_srv = rospy.ServiceProxy( # "panda_moveit_planner_server/planning_scene/add_box",