diff --git a/armer/armer.py b/armer/armer.py index 23e3345..8a72e7a 100644 --- a/armer/armer.py +++ b/armer/armer.py @@ -242,15 +242,19 @@ def load(path: str) -> Armer: logging=logging ) - def global_collision_check(self, robot: ROSRobot): - """ - Conducts a full check for collisions + def global_collision_check(self, robot: ROSRobot) -> bool: + """Conducts a full check for collisions NOTE: takes a given robot object and runs its collision check (of its own dictionary) against the global dictionary - the global dictionary may have collision data from multiple robots (with different link data) + the global dictionary may have collision data from multiple robots (with different link data) TODO: currently each robot is checked against its own link data. This is needed for self collision checking - but could be possibly optimised in some way as to not be overloaded with multiple instances + but could be possibly optimised in some way as to not be overloaded with multiple instances NOTE: [2023-10-31] Identified that this component is very inefficient for the panda (real test). Implemented - a start and stop link (e.g., terminating search from end-effector to panda_link8, rather than full tree) + a start and stop link (e.g., terminating search from end-effector to panda_link8, rather than full tree) + + :param robot: ROSRobot object for checking against + :type robot: ROSRobot + :return: True if collision is found, else False + :rtype: bool """ # Error handling on gripper name if robot.gripper == None or robot.gripper == "": @@ -323,11 +327,12 @@ def global_collision_check(self, robot: ROSRobot): # No collisions found with no errors identified. return False - def archived_global_collision_check(self, robot: ROSRobot): - """ - Conducts a full check for collisions - NOTE: [2023-11-03] archieved as cython implementation is being used - kept for debugging purposes + def archived_global_collision_check(self, robot: ROSRobot) -> bool: + """Conducts a full check for collisions + NOTE: [2023-11-03] archived as cython implementation is being used - kept for debugging purposes + + :return: True if collision is found, or False + :rtype: bool """ # Error handling on gripper name if robot.gripper == None or robot.gripper == "": diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py index bc0aac1..52078e9 100644 --- a/armer/robots/ROSRobot.py +++ b/armer/robots/ROSRobot.py @@ -606,6 +606,11 @@ def __init__(self, # --------- ROS Topic Callback Methods -------------------------------- # # --------------------------------------------------------------------- # def _state_cb(self, msg): + """Updates the current joint state of the robot (from hardware) + + :param msg: Message containing joint data + :type msg: JointState + """ if not self.joint_indexes: for joint_name in self.joint_names: self.joint_indexes.append(msg.name.index(joint_name)) @@ -614,8 +619,7 @@ def _state_cb(self, msg): self.joint_states = msg def velocity_cb(self, msg: TwistStamped) -> None: - """ - ROS velocity callback: + """ROS velocity callback: Moves the arm at the specified cartesian velocity w.r.t. a target frame @@ -634,8 +638,7 @@ def velocity_cb(self, msg: TwistStamped) -> None: self.__vel_move(msg) def joint_velocity_cb(self, msg: JointVelocity) -> None: - """ - ROS joint velocity callback: + """ROS joint velocity callback: Moves the joints of the arm at the specified velocities :param msg: [description] @@ -661,8 +664,7 @@ def joint_velocity_cb(self, msg: JointVelocity) -> None: # --------- Traditional ROS Action Callback Methods ------------------- # # --------------------------------------------------------------------- # def guarded_velocity_cb(self, msg: GuardedVelocityGoal) -> None: - """ - ROS Guarded velocity callback + """ROS Guarded velocity callback Moves the end-effector in cartesian space with respect to guards (time or force) :param msg: [description] @@ -692,8 +694,7 @@ def guarded_velocity_cb(self, msg: GuardedVelocityGoal) -> None: self.velocity_server.set_aborted(GuardedVelocityResult()) def servo_cb(self, msg) -> None: - """ - ROS Servoing Action Callback: + """ROS Servoing Action Callback: Servos the end-effector to the cartesian pose given by msg :param msg: [description] @@ -764,8 +765,7 @@ def servo_cb(self, msg) -> None: self.cartesian_servo_publisher.publish(arrived) def pose_cb(self, goal: MoveToPoseGoal) -> None: - """ - ROS Action Server callback: + """ROS Action Server callback: Moves the end-effector to the cartesian pose indicated by goal @@ -807,12 +807,10 @@ def pose_cb(self, goal: MoveToPoseGoal) -> None: self.moving = False def joint_pose_cb(self, goal: MoveToJointPoseGoal) -> None: - """ - ROS Action Server callback: + """ROS Action Server callback: Moves the arm the named pose indicated by goal - :param goal: Goal message containing the name of - the joint configuration to which the arm should move + :param goal: Goal message containing the name of the joint configuration to which the arm should move :type goal: MoveToNamedPoseGoal """ if self.moving: @@ -831,12 +829,10 @@ def joint_pose_cb(self, goal: MoveToJointPoseGoal) -> None: self.moving = False def named_pose_cb(self, goal: MoveToNamedPoseGoal) -> None: - """ - ROS Action Server callback: + """ROS Action Server callback: Moves the arm the named pose indicated by goal - :param goal: Goal message containing the name of - the joint configuration to which the arm should move + :param goal: Goal message containing the name of the joint configuration to which the arm should move :type goal: MoveToNamedPoseGoal """ if self.moving: @@ -1674,13 +1670,11 @@ def calibrate_transform_cb(self, req: CalibrateTransformRequest) -> CalibrateTra def set_cartesian_impedance_cb( # pylint: disable=no-self-use self, request: SetCartesianImpedanceRequest) -> SetCartesianImpedanceResponse: - """ - ROS Service Callback + """ROS Service Callback Set the 6-DOF impedance of the end-effector. Higher values should increase the stiffness of the robot while lower values should increase compliance - :param request: The numeric values representing the EE impedance (6-DOF) that - should be set on the arm + :param request: The numeric values representing the EE impedance (6-DOF) that should be set on the arm :type request: GetNamedPoseConfigsRequest :return: True if the impedence values were updated successfully :rtype: GetNamedPoseConfigsResponse @@ -1903,16 +1897,20 @@ def enable_collision_debug_cb(self, req: SetBoolRequest) -> SetBoolResponse: return SetBoolResponse(success=True, message="Collision Debug is Disabled") def add_collision_obj_cb(self, req: AddCollisionObjectRequest) -> AddCollisionObjectResponse: - """ - Adds a collision primative (sphere, cylinder, cuboid) to existing collision dictionary at runtime + """Adds a collision primative (sphere, cylinder, cuboid) to existing collision dictionary at runtime Expected input: - -> name (string): to define the key within the collision dictionary - -> type (string): to define basic primatives (as part of the sg.Shape class) - -> TODO: base_link (string): link name to attach object to - -> radius (float): used to define radius of sphere or cylinder - -> length (float): used to define length of cylinder - -> pose (Pose): to define the shape's location - -> overwrite (bool): True means the same key name (if in the dictionary) can be overwritten + -> name (string): to define the key within the collision dictionary + -> type (string): to define basic primatives (as part of the sg.Shape class) + -> TODO: base_link (string): link name to attach object to + -> radius (float): used to define radius of sphere or cylinder + -> length (float): used to define length of cylinder + -> pose (Pose): to define the shape's location + -> overwrite (bool): True means the same key name (if in the dictionary) can be overwritten + + :param req: Service request for name, type of object, radius, length or scale, as well as pose + :type req: AddCollisionObjectRequest + :return: Service response for success (True) or failure (False) + :rtype: AddCollisionObjectResponse """ # Handle early termination on input error for name and type if req.name == None or req.name == ''\ @@ -1997,10 +1995,9 @@ def add_collision_obj_cb(self, req: AddCollisionObjectRequest) -> AddCollisionOb return AddCollisionObjectResponse(success=True) def remove_collision_obj_cb(self, req: RemoveCollisionObjectRequest) -> RemoveCollisionObjectResponse: - """ - This will take a given key and (if it exists) and removes said key object as a collision shape + """This will take a given key and (if it exists) and removes said key object as a collision shape NOTE: currently expects the following - -> name (string) to access object in question + -> name (string) to access object in question """ # Handle early termination on input error for name and type if req.name == None or req.name == '': @@ -2429,11 +2426,10 @@ def check_collision_per_state(self, q: list() = []) -> bool(): return go_signal def update_link_collision_window(self): - """ - This method updates a sliced list of links (member variable) + """This method updates a sliced list of links (member variable) as determined by the class method variables: - collision_check_start_link - collision_check_stop_link + - collision_check_start_link + - collision_check_stop_link """ with Timer("Link Slicing Check", enabled=False): # Prepare sliced link based on a defined stop link @@ -2470,13 +2466,16 @@ def update_link_collision_window(self): self.collision_sliced_links.reverse() def characterise_collision_overlaps(self, debug: bool = False) -> bool: - """ - Characterises the existing robot tree and tracks overlapped links in collision handling + """Characterises the existing robot tree and tracks overlapped links in collision handling NOTE: needed to do collision checking, when joints are typically (neighboring) overlapped - NOTE: this is quite an intensive run at the moment, - however, it is only expected to be run in single intervals (not continuous) - [2023-10-27] approx. time frequency is 1hz (Panda simulated) - [2023-10-31] approx. time frequency is 40Hz and 21Hz (UR10 and Panda simulated with better method, respectively) + NOTE: this is quite an intensive run at the moment, however, it is only expected to be run in single intervals (not continuous) + - [2023-10-27] approx. time frequency is 1hz (Panda simulated) + - [2023-10-31] approx. time frequency is 40Hz and 21Hz (UR10 and Panda simulated with better method, respectively) + + :param debug: True to output debugging information on collision overlaps, defaults to False + :type debug: bool, optional + :return: True if successfully completed or False if in error + :rtype: bool """ # Running timer to get frequency of run. Set enabled to True for debugging output to stdout with Timer(name="Characterise Collision Overlaps", enabled=True): @@ -2916,14 +2915,23 @@ def write_collision_scene_config(self, config_path: str = '') -> bool: # --------------------------------------------------------------------- # # --------- Standard Methods ------------------------------------------ # # --------------------------------------------------------------------- # - def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False, workspace_ignore: bool = False): - """ - A general executor that performs the following on a given joint state goal + def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False, workspace_ignore: bool = False) -> bool: + """A general executor that performs the following on a given joint state goal - Workspace check prior to move. Setting workspace_ignore to True skips this (for cases where a pose is not defined or needed) - Singularity checking and termination on failure - Collision checking and termination on failure. Setting collision_ignore to True skips this check - - Execution of a ros_control or standard execution (real/sim) depending on what - is available + - Execution of a ros_control or standard execution (real/sim) depending on what is available + + :param q: array like joint state of the arm + :type q: np.ndarray + :param pose: Pose requested to move to, defaults to None + :type pose: Pose, optional + :param collision_ignore: True to ignore collision checking, defaults to False + :type collision_ignore: bool, optional + :param workspace_ignore: True to ignore workspace checking, defaults to False + :type workspace_ignore: bool, optional + :return: True on success or False + :rtype: bool """ result = False if self.pose_within_workspace(pose) == False and not workspace_ignore: @@ -3397,14 +3405,12 @@ def normalizeQuaternion( self, quaternion_msg ): quaternion_msg.w *= s def interactive_marker_creation(self): - """ - NOTE: updated and implemented from the tutorials here: - https://github.com/ros-visualization/visualization_tutorials/tree/noetic-devel + """NOTE: updated and implemented from the tutorials here: https://github.com/ros-visualization/visualization_tutorials/tree/noetic-devel Publishes interactive marker versions of added shape objects Currently handles only: - - Sphere - - Cylinder - - Cuboid + - Sphere + - Cylinder + - Cuboid """ dyn_col_dict_cp = self.dynamic_collision_dict.copy() for obj in list(dyn_col_dict_cp.values()): diff --git a/docs/armer_installation.rst b/docs/armer_installation.rst index 8f1b82d..cf9f7d5 100644 --- a/docs/armer_installation.rst +++ b/docs/armer_installation.rst @@ -10,13 +10,13 @@ Requirements * `QCR repos `_ Robot Specific Requirements -------------- +--------------------------- * ROS drivers with joint group velocity controllers (ros_control) * Robotics Toolbox model or URDF (loaded as a robot_description parameter) Quickstart Installation (Native Linux) --------------------------------- +-------------------------------------- Copy and paste the following code snippet into a terminal to create a new catkin workspace and install the Armer drivers to it. Note this script will also add the workspace to be sourced every time a bash terminal is opened. @@ -45,7 +45,7 @@ Copy and paste the following code snippet into a terminal to create a new catkin If the ``rosdep`` command is failing to find dependencies, make sure the QCR Robotic Vision repos have been added. See `Adding QCR Robotic Vision Repos `_ RoboStack Installation (Linux, Mac, Windows) --------------------------------- +-------------------------------------------- To enable easy use of ROS on these operating systems, it is recommended to use `RoboStack `_; note that ROS 1 (noetic) is recommended at this stage. Please ensure you have `mamba `_ installed before proceeding. Please follow all required steps for the RoboStack install (as per their instructions) to enable the smoothest setup on your particular OS. diff --git a/docs/collision_handling.rst b/docs/collision_handling.rst index 73a072d..dad875f 100644 --- a/docs/collision_handling.rst +++ b/docs/collision_handling.rst @@ -5,31 +5,9 @@ Armer implements collision handling features that extend the capabilities provid A description of each core functionality is provided below. URDF Parsed Collision Objects ---------------------- +------------------------------ -Simply navigate a manipulator into any pose by any means and call the service ``/arm/set_named_pose``. The pose is saved locally to a config file in YAML format and automatically loaded each time Armer is launched. -The service can be summoned via Python: - -.. code-block:: python - - from armer_msgs.srv import AddNamedPose - import rospy - - rospy.init_node('armer_example', disable_signals=True) - add_pose_service = rospy.ServiceProxy('/arm/set_named_pose', AddNamedPose) - - named_pose="my_pose" - add_pose_service(named_pose, True) - - -Or via bash command line: - -.. code-block:: bash - - rosservice call /arm/set_named_pose "pose_name: {DESIRED_POSE_NAME}" - -The pose is now saved and the robot can navigate to the saved pose by making a request to the action server. Moving to a Named Pose ------------------------