From b46616842d7215b58c1c203f2585d6e9529cf09c Mon Sep 17 00:00:00 2001 From: Rick Staa Date: Wed, 22 Nov 2023 18:56:25 +0100 Subject: [PATCH] feat: make code compatible with latest franka_ros version (#183) This commit guarantees that the `panda_gazebo` code aligns with the most recent upstream commit (specifically 5f90395ef000e0c998fb7aff8f127b9bd3773962). Adjustments were made to the joint position and joint effort control services to integrate with the updated version seamlessly. In the new iteration, the panda arm joint positions and efforts are managed through group controllers. You can find additional insights at https://github.com/frankaemika/franka_ros/pull/181#issuecomment-1064120809. --- franka_ros | 2 +- .../effort_joint_trajectory_controllers.yaml | 3 +- .../controllers/joint_effort_controllers.yaml | 23 -- .../joint_group_effort_controller.yaml | 12 + .../joint_group_position_controller.yaml | 12 + .../joint_position_controllers.yaml | 23 -- ...position_joint_trajectory_controllers.yaml | 2 + .../dyn_reconf/EffortControlTestDynReconf.cfg | 4 +- .../PositionControlTestDynReconf.cfg | 4 +- .../docs/source/get_started/usage.rst | 8 +- .../launch/load_controllers.launch.xml | 12 +- ...rt_joint_trajectory_controller.launch.xml} | 4 +- .../load_joint_effort_controllers.launch.xml | 13 - ...d_joint_group_effort_controller.launch.xml | 12 + ...joint_group_position_controller.launch.xml | 12 + ...load_joint_position_controllers.launch.xml | 13 - ...on_joint_trajectory_controller.launch.xml} | 4 +- panda_gazebo/launch/panda_rviz.launch.xml | 10 +- panda_gazebo/launch/put_robot_in_world.launch | 10 +- .../launch/start_pick_and_place_world.launch | 4 +- panda_gazebo/launch/start_push_world.launch | 4 +- panda_gazebo/launch/start_reach_world.launch | 4 +- panda_gazebo/launch/start_slide_world.launch | 4 +- panda_gazebo/launch/start_world.launch.xml | 4 +- panda_gazebo/package.xml | 3 +- .../tools/inertia_mass_calculator.py | 2 - .../resources/tools/inertial_xml_marker.py | 3 +- ...joint_effort_dynamic_reconfigure_server.py | 118 ------ ...oint_efforts_dynamic_reconfigure_server.py | 127 +++++++ ...int_position_dynamic_reconfigure_server.py | 117 ------ ...nt_positions_dynamic_reconfigure_server.py | 125 +++++++ panda_gazebo/setup.cfg | 2 + panda_gazebo/src/panda_gazebo/__init__.py | 2 +- .../common/controlled_joints_dict.py | 5 +- .../common/controller_info_dict.py | 5 +- .../src/panda_gazebo/core/__init__.py | 1 - .../src/panda_gazebo/core/control_server.py | 342 ++++++++---------- .../src/panda_gazebo/core/control_switcher.py | 2 +- .../src/panda_gazebo/core/group_publisher.py | 70 ---- panda_gazebo/src/panda_gazebo/version.py | 2 +- ...joint_effort_dynamic_reconfigure_server.py | 113 ------ ...int_position_dynamic_reconfigure_server.py | 112 ------ .../tests/manual/moveit_commander_test.py | 114 +++--- .../moveit_server_dynamic_reconfigure_test.py | 5 +- .../tests/manual/panda_control_server_test.py | 3 +- setup.cfg | 3 + 46 files changed, 564 insertions(+), 915 deletions(-) delete mode 100644 panda_gazebo/cfg/controllers/joint_effort_controllers.yaml create mode 100644 panda_gazebo/cfg/controllers/joint_group_effort_controller.yaml create mode 100644 panda_gazebo/cfg/controllers/joint_group_position_controller.yaml delete mode 100644 panda_gazebo/cfg/controllers/joint_position_controllers.yaml rename panda_gazebo/launch/{load_effort_joint_trajectory_controllers.launch.xml => load_effort_joint_trajectory_controller.launch.xml} (87%) delete mode 100755 panda_gazebo/launch/load_joint_effort_controllers.launch.xml create mode 100755 panda_gazebo/launch/load_joint_group_effort_controller.launch.xml create mode 100755 panda_gazebo/launch/load_joint_group_position_controller.launch.xml delete mode 100755 panda_gazebo/launch/load_joint_position_controllers.launch.xml rename panda_gazebo/launch/{load_position_joint_trajectory_controllers.launch.xml => load_position_joint_trajectory_controller.launch.xml} (86%) delete mode 100755 panda_gazebo/scripts/joint_effort_dynamic_reconfigure_server.py create mode 100755 panda_gazebo/scripts/joint_efforts_dynamic_reconfigure_server.py delete mode 100755 panda_gazebo/scripts/joint_position_dynamic_reconfigure_server.py create mode 100755 panda_gazebo/scripts/joint_positions_dynamic_reconfigure_server.py delete mode 100644 panda_gazebo/src/panda_gazebo/core/group_publisher.py delete mode 100755 panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server.py delete mode 100755 panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server.py diff --git a/franka_ros b/franka_ros index 308a06cf..3bee2b75 160000 --- a/franka_ros +++ b/franka_ros @@ -1 +1 @@ -Subproject commit 308a06cf2084677e42ea84f3993b5886d0e9cc1c +Subproject commit 3bee2b754dec9c27fb431cafa5abf5c42cf159ad diff --git a/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml b/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml index 5c0b5a55..eb73f201 100644 --- a/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml +++ b/panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml @@ -1,5 +1,7 @@ # Configuration file that contains the configuration values for setting up the panda # trajectory controllers that can control a joint effort hardware interface. +# NOTE: Equal to Franka's 'effort_joint_trajectory_controller' but with a different +# goal tolerance (see https://github.com/frankaemika/franka_ros/issues/201). panda_arm_controller: type: effort_controllers/JointTrajectoryController joints: @@ -20,7 +22,6 @@ panda_arm_controller: panda_joint7: { p: 50, d: 5, i: 0 } constraints: goal_time: 0.5 - stopped_velocity_tolerance: 0 # Added because of wrong PID gains see #9 panda_joint1: { goal: 0.005 } panda_joint2: { goal: 0.005 } panda_joint3: { goal: 0.005 } diff --git a/panda_gazebo/cfg/controllers/joint_effort_controllers.yaml b/panda_gazebo/cfg/controllers/joint_effort_controllers.yaml deleted file mode 100644 index bf7340a7..00000000 --- a/panda_gazebo/cfg/controllers/joint_effort_controllers.yaml +++ /dev/null @@ -1,23 +0,0 @@ -# Configuration file that contains the configuration values for setting up the panda -# effort controllers. -panda_arm_joint1_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_joint1 -panda_arm_joint2_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_joint2 -panda_arm_joint3_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_joint3 -panda_arm_joint4_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_joint4 -panda_arm_joint5_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_joint5 -panda_arm_joint6_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_joint6 -panda_arm_joint7_effort_controller: - type: effort_controllers/JointEffortController - joint: panda_joint7 diff --git a/panda_gazebo/cfg/controllers/joint_group_effort_controller.yaml b/panda_gazebo/cfg/controllers/joint_group_effort_controller.yaml new file mode 100644 index 00000000..b3f5a8c5 --- /dev/null +++ b/panda_gazebo/cfg/controllers/joint_group_effort_controller.yaml @@ -0,0 +1,12 @@ +# Configuration file that contains the configuration values for setting up the panda +# group effort controller. +panda_arm_joint_effort_controller: + type: effort_controllers/JointGroupEffortController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 diff --git a/panda_gazebo/cfg/controllers/joint_group_position_controller.yaml b/panda_gazebo/cfg/controllers/joint_group_position_controller.yaml new file mode 100644 index 00000000..52b4cdab --- /dev/null +++ b/panda_gazebo/cfg/controllers/joint_group_position_controller.yaml @@ -0,0 +1,12 @@ +# Configuration file that contains the configuration values for setting up the panda +# group position controller. +panda_arm_joint_position_controller: + type: position_controllers/JointGroupPositionController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 diff --git a/panda_gazebo/cfg/controllers/joint_position_controllers.yaml b/panda_gazebo/cfg/controllers/joint_position_controllers.yaml deleted file mode 100644 index 9f9c3907..00000000 --- a/panda_gazebo/cfg/controllers/joint_position_controllers.yaml +++ /dev/null @@ -1,23 +0,0 @@ -# Configuration file that contains the configuration values for setting up the panda -# position controllers. -panda_arm_joint1_position_controller: - type: position_controllers/JointPositionController - joint: panda_joint1 -panda_arm_joint2_position_controller: - type: position_controllers/JointPositionController - joint: panda_joint2 -panda_arm_joint3_position_controller: - type: position_controllers/JointPositionController - joint: panda_joint3 -panda_arm_joint4_position_controller: - type: position_controllers/JointPositionController - joint: panda_joint4 -panda_arm_joint5_position_controller: - type: position_controllers/JointPositionController - joint: panda_joint5 -panda_arm_joint6_position_controller: - type: position_controllers/JointPositionController - joint: panda_joint6 -panda_arm_joint7_position_controller: - type: position_controllers/JointPositionController - joint: panda_joint7 diff --git a/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml b/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml index 8d241111..ce25ef47 100644 --- a/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml +++ b/panda_gazebo/cfg/controllers/position_joint_trajectory_controllers.yaml @@ -1,5 +1,7 @@ # Configuration file that contains the configuration values for setting up the panda # trajectory controllers. +# NOTE: Equal to Franka's 'position_joint_trajectory_controller' but with a different +# goal tolerance (see https://github.com/frankaemika/franka_ros/issues/201). panda_arm_controller: type: position_controllers/JointTrajectoryController joints: diff --git a/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg b/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg index 916a087d..bc38db75 100755 --- a/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg +++ b/panda_gazebo/cfg/dyn_reconf/EffortControlTestDynReconf.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# NOTE: Config for changing the joint efforts. +# NOTE: Config for changing the panda arm joint efforts and gripper width and speed. PACKAGE = "panda_gazebo" from dynamic_reconfigure.parameter_generator_catkin import (ParameterGenerator, @@ -39,4 +39,4 @@ hand.add( ) # Generate the necessary files and exit the program. -exit(gen.generate(PACKAGE, "panda_test", "JointEffort")) +exit(gen.generate(PACKAGE, "panda_test", "JointEfforts")) diff --git a/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg b/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg index 66dcd0cc..de105aa5 100755 --- a/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg +++ b/panda_gazebo/cfg/dyn_reconf/PositionControlTestDynReconf.cfg @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# NOTE: Config for the changing the joint positions +# NOTE: Config for the changing the panda arm joint positions and gripper width and speed. PACKAGE = "panda_gazebo" from dynamic_reconfigure.parameter_generator_catkin import (ParameterGenerator, @@ -39,4 +39,4 @@ hand.add( ) # Generate the necessary files and exit the program. -exit(gen.generate(PACKAGE, "panda_test", "JointPosition")) +exit(gen.generate(PACKAGE, "panda_test", "JointPositions")) diff --git a/panda_gazebo/docs/source/get_started/usage.rst b/panda_gazebo/docs/source/get_started/usage.rst index 3e8d98bc..1ecfa868 100644 --- a/panda_gazebo/docs/source/get_started/usage.rst +++ b/panda_gazebo/docs/source/get_started/usage.rst @@ -24,16 +24,18 @@ modes that can be selected using the ``control_mode`` argument: .. Note:: - You can test different control modes using the :mod:`joint_effort_dynamic_reconfigure_server` and :mod:`joint_position_dynamic_reconfigure_server` nodes. + You can test different control modes using the :mod:`joint_efforts_dynamic_reconfigure_server` and :mod:`joint_positions_dynamic_reconfigure_server` nodes. These nodes allow you to send joint efforts and joint positions to the robot. To learn more about utilizing these dynamic reconfigure servers, refer to the documentation of the `dynamic_reconfigure`_ and `rqt_reconfigure`_ ROS packages. - Furthermore, you can explore trajectory control using the `MoveIt! package`_. To enable this functionality, set the ``use_moveit`` launch file argument to ```true``. Once - enabled, you can control the robot through the `RViz Motion Planning`_ panel. For detailed instructions on how to use `MoveIt!`_, consult the `MoveIt! tutorials`_. + Furthermore, you can explore trajectory control using the `MoveIt! package`_ or `rqt_joint_trajectory_controller package`_. To enable `MoveIt!`, set the + ``use_moveit`` launch file argument to ``true``. Once enabled, you can control the robot through the `RViz Motion Planning`_ panel. For detailed instructions on how to + use `MoveIt!`_, consult the `MoveIt! tutorials`_. .. _dynamic_reconfigure: https://wiki.ros.org/dynamic_reconfigure .. _rqt_reconfigure: https://wiki.ros.org/rqt_reconfigure .. _`MoveIt! package`: https://moveit.ros.org/ +.. _`rqt_joint_trajectory_controller package`: https://wiki.ros.org/rqt_joint_trajectory_controller .. _`RViz Motion Planning`: https://ros-planning.github.io/moveit_tutorials/doc/motion_planning_rviz/motion_planning_rviz_tutorial.html .. _`MoveIt!`: https://ros-planning.github.io/moveit_tutorials/ .. _`MoveIt! tutorials`: https://ros-planning.github.io/moveit_tutorials/ diff --git a/panda_gazebo/launch/load_controllers.launch.xml b/panda_gazebo/launch/load_controllers.launch.xml index afc97cd4..919a5271 100644 --- a/panda_gazebo/launch/load_controllers.launch.xml +++ b/panda_gazebo/launch/load_controllers.launch.xml @@ -1,4 +1,4 @@ - + @@ -18,20 +18,20 @@ - + - + - + - + - \ No newline at end of file + diff --git a/panda_gazebo/launch/load_effort_joint_trajectory_controllers.launch.xml b/panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml similarity index 87% rename from panda_gazebo/launch/load_effort_joint_trajectory_controllers.launch.xml rename to panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml index 60fa9d74..2360b1e7 100755 --- a/panda_gazebo/launch/load_effort_joint_trajectory_controllers.launch.xml +++ b/panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml @@ -1,4 +1,4 @@ - + @@ -10,4 +10,4 @@ - \ No newline at end of file + diff --git a/panda_gazebo/launch/load_joint_effort_controllers.launch.xml b/panda_gazebo/launch/load_joint_effort_controllers.launch.xml deleted file mode 100755 index aaf66ad0..00000000 --- a/panda_gazebo/launch/load_joint_effort_controllers.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/panda_gazebo/launch/load_joint_group_effort_controller.launch.xml b/panda_gazebo/launch/load_joint_group_effort_controller.launch.xml new file mode 100755 index 00000000..3a8dfe20 --- /dev/null +++ b/panda_gazebo/launch/load_joint_group_effort_controller.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/panda_gazebo/launch/load_joint_group_position_controller.launch.xml b/panda_gazebo/launch/load_joint_group_position_controller.launch.xml new file mode 100755 index 00000000..6e871d5e --- /dev/null +++ b/panda_gazebo/launch/load_joint_group_position_controller.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/panda_gazebo/launch/load_joint_position_controllers.launch.xml b/panda_gazebo/launch/load_joint_position_controllers.launch.xml deleted file mode 100755 index b375772d..00000000 --- a/panda_gazebo/launch/load_joint_position_controllers.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/panda_gazebo/launch/load_position_joint_trajectory_controllers.launch.xml b/panda_gazebo/launch/load_position_joint_trajectory_controller.launch.xml similarity index 86% rename from panda_gazebo/launch/load_position_joint_trajectory_controllers.launch.xml rename to panda_gazebo/launch/load_position_joint_trajectory_controller.launch.xml index 0780204b..5a2a33a8 100755 --- a/panda_gazebo/launch/load_position_joint_trajectory_controllers.launch.xml +++ b/panda_gazebo/launch/load_position_joint_trajectory_controller.launch.xml @@ -1,4 +1,4 @@ - + @@ -9,4 +9,4 @@ - \ No newline at end of file + diff --git a/panda_gazebo/launch/panda_rviz.launch.xml b/panda_gazebo/launch/panda_rviz.launch.xml index 86e4fea9..cc88b333 100755 --- a/panda_gazebo/launch/panda_rviz.launch.xml +++ b/panda_gazebo/launch/panda_rviz.launch.xml @@ -1,18 +1,18 @@ - + - + - + - + - \ No newline at end of file + diff --git a/panda_gazebo/launch/put_robot_in_world.launch b/panda_gazebo/launch/put_robot_in_world.launch index aa9a32ec..ca9b8fa8 100644 --- a/panda_gazebo/launch/put_robot_in_world.launch +++ b/panda_gazebo/launch/put_robot_in_world.launch @@ -12,7 +12,7 @@ - + @@ -23,15 +23,13 @@ + + - - - - @@ -40,13 +38,13 @@ - + diff --git a/panda_gazebo/launch/start_pick_and_place_world.launch b/panda_gazebo/launch/start_pick_and_place_world.launch index ce332121..826c7e92 100644 --- a/panda_gazebo/launch/start_pick_and_place_world.launch +++ b/panda_gazebo/launch/start_pick_and_place_world.launch @@ -1,4 +1,4 @@ - + @@ -16,4 +16,4 @@ - \ No newline at end of file + diff --git a/panda_gazebo/launch/start_push_world.launch b/panda_gazebo/launch/start_push_world.launch index eec51ed6..f1fc1fe8 100644 --- a/panda_gazebo/launch/start_push_world.launch +++ b/panda_gazebo/launch/start_push_world.launch @@ -1,4 +1,4 @@ - + @@ -16,4 +16,4 @@ - \ No newline at end of file + diff --git a/panda_gazebo/launch/start_reach_world.launch b/panda_gazebo/launch/start_reach_world.launch index 50481f8a..9b3a0a6b 100644 --- a/panda_gazebo/launch/start_reach_world.launch +++ b/panda_gazebo/launch/start_reach_world.launch @@ -1,4 +1,4 @@ - + @@ -16,4 +16,4 @@ - \ No newline at end of file + diff --git a/panda_gazebo/launch/start_slide_world.launch b/panda_gazebo/launch/start_slide_world.launch index c717094b..5df1e3fd 100644 --- a/panda_gazebo/launch/start_slide_world.launch +++ b/panda_gazebo/launch/start_slide_world.launch @@ -1,4 +1,4 @@ - + @@ -16,4 +16,4 @@ - \ No newline at end of file + diff --git a/panda_gazebo/launch/start_world.launch.xml b/panda_gazebo/launch/start_world.launch.xml index b29be001..2331736d 100644 --- a/panda_gazebo/launch/start_world.launch.xml +++ b/panda_gazebo/launch/start_world.launch.xml @@ -1,4 +1,4 @@ - + @@ -15,4 +15,4 @@ - \ No newline at end of file + diff --git a/panda_gazebo/package.xml b/panda_gazebo/package.xml index b13de510..e9ca0103 100755 --- a/panda_gazebo/package.xml +++ b/panda_gazebo/package.xml @@ -89,9 +89,10 @@ joint_trajectory_controller + franka_description diff --git a/panda_gazebo/resources/tools/inertia_mass_calculator.py b/panda_gazebo/resources/tools/inertia_mass_calculator.py index 8f5a08bc..33df18fa 100644 --- a/panda_gazebo/resources/tools/inertia_mass_calculator.py +++ b/panda_gazebo/resources/tools/inertia_mass_calculator.py @@ -43,7 +43,6 @@ def get_cube_inertia(height, width, depth, mass): Returns: numpy.ndarray: The inertia matrix [kg*m^2]. """ - return np.array( [ [(1 / 12) * mass * (width**2 + depth**2), 0, 0], @@ -79,7 +78,6 @@ def get_cylinder_inertia(radius, height, mass): Returns: numpy.ndarray: The inertia matrix [kg*m^2]. """ - return np.array( [ [(1 / 12) * mass * (3 * radius**2 + height**2), 0, 0], diff --git a/panda_gazebo/resources/tools/inertial_xml_marker.py b/panda_gazebo/resources/tools/inertial_xml_marker.py index 38c947f1..a13f157f 100644 --- a/panda_gazebo/resources/tools/inertial_xml_marker.py +++ b/panda_gazebo/resources/tools/inertial_xml_marker.py @@ -1,5 +1,6 @@ """This script creates gazebo inertia xml code when it is supplied with the joint mass, -inertia and center of mass.""" +inertia and center of mass. +""" # Make script python3 compatible from __future__ import print_function diff --git a/panda_gazebo/scripts/joint_effort_dynamic_reconfigure_server.py b/panda_gazebo/scripts/joint_effort_dynamic_reconfigure_server.py deleted file mode 100755 index 15d98237..00000000 --- a/panda_gazebo/scripts/joint_effort_dynamic_reconfigure_server.py +++ /dev/null @@ -1,118 +0,0 @@ -#!/usr/bin/env python3 -"""Small node that spins up a dynamic reconfigure server that can be used to change the -joint efforts. -""" -import actionlib -import rospy -from dynamic_reconfigure.server import Server -from franka_gripper.msg import MoveAction, MoveGoal -from sensor_msgs.msg import JointState -from std_msgs.msg import Float64 - -from panda_gazebo.cfg import JointEffortConfig - - -class JointEffortDynamicReconfigureServer: - """A small node that spins up a dynamic reconfigure server that can be used to - change the joint efforts. - """ - - def __init__(self): - """Initialise JointEffortDynamicReconfigureServer object.""" - self.srv = Server(JointEffortConfig, self.callback) - - # Create joint effort publishers. - self.arm_joint1_pub = rospy.Publisher( - "panda_arm_joint1_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint2_pub = rospy.Publisher( - "panda_arm_joint2_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint3_pub = rospy.Publisher( - "panda_arm_joint3_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint4_pub = rospy.Publisher( - "panda_arm_joint4_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint5_pub = rospy.Publisher( - "panda_arm_joint5_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint6_pub = rospy.Publisher( - "panda_arm_joint6_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint7_pub = rospy.Publisher( - "panda_arm_joint7_effort_controller/command", Float64, queue_size=10 - ) - self.arm_pubs = [ - self.arm_joint1_pub, - self.arm_joint2_pub, - self.arm_joint3_pub, - self.arm_joint4_pub, - self.arm_joint5_pub, - self.arm_joint6_pub, - self.arm_joint7_pub, - ] - self.gripper_move_client = actionlib.SimpleActionClient( - "franka_gripper/move", MoveAction - ) - self.gripper_connected = self.gripper_move_client.wait_for_server( - timeout=rospy.Duration(secs=5) - ) - - def callback(self, config, level): - """Dynamic reconfigure callback function. - - Args: - config (:obj:`dynamic_reconfigure.encoding.Config`): The current dynamic - reconfigure configuration object. - level (int): Bitmask that gives information about which parameter has been - changed. - - Returns: - :obj:`~dynamic_reconfigure.encoding.Config`: Modified dynamic reconfigure - configuration object. - """ - rospy.loginfo( - ( - "Reconfigure Request: {joint1_effort}, {joint2_effort}, " - "{joint3_effort} {joint4_effort}, {joint5_effort}, {joint6_effort}, " - "{joint7_effort}, {width}, {speed}" - ).format(**config) - ) - - # Set initial joint states. - if level == -1: - joint_states = rospy.wait_for_message("joint_states", JointState) - position_dict = dict(zip(joint_states.name, joint_states.position)) - effort_dict = dict(zip(joint_states.name, joint_states.effort)) - set_values = list(effort_dict.values())[:-2] - set_values.append(list(position_dict.values())[-1] * 2) - config.update( - **dict( - zip([key for key in config.keys() if key != "groups"], set_values) - ) - ) - - # Write joint positions to controller topics. - if level > -1 and level < 7: - self.arm_pubs[level].publish(list(config.values())[level]) - elif level in [7, 8] and self.gripper_connected: - move_goal = MoveGoal(width=config["width"], speed=config["speed"]) - self.gripper_move_client.send_goal(move_goal) - result = self.gripper_move_client.wait_for_result() - if not result: - rospy.logerr("Something went wrong while setting the gripper commands.") - elif level in [7, 8] and not self.gripper_connected: - rospy.logwarn_once( - "Gripper commands not applied since the gripper command action was not " - "found." - ) - return config - - -if __name__ == "__main__": - rospy.init_node("joint_effort_reconfig_server", anonymous=False) - - effort_dyn_server = JointEffortDynamicReconfigureServer() - - rospy.spin() diff --git a/panda_gazebo/scripts/joint_efforts_dynamic_reconfigure_server.py b/panda_gazebo/scripts/joint_efforts_dynamic_reconfigure_server.py new file mode 100755 index 00000000..cee6e0f3 --- /dev/null +++ b/panda_gazebo/scripts/joint_efforts_dynamic_reconfigure_server.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 +"""Small node that spins up a dynamic reconfigure server that can be used to change the +panda arm joint efforts and gripper width. +""" +import actionlib +import rospy +from dynamic_reconfigure.server import Server +from franka_gripper.msg import MoveAction, MoveGoal +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64MultiArray + +from panda_gazebo.cfg import JointEffortsConfig + +# Constants for topic names +ARM_TOPIC = "/panda_arm_joint_effort_controller/command" +JOINT_STATES_TOPIC = "joint_states" +GRIPPER_ACTION_NAME = "franka_gripper/move" + + +class JointEffortsDynamicReconfigureServer: + """A small node that spins up a dynamic reconfigure server that can be used to + change the panda arm joint efforts and gripper width. + """ + + def __init__(self): + """Initialise JointEffortsDynamicReconfigureServer object.""" + rospy.loginfo("Starting dynamic reconfigure server...") + self.srv = Server(JointEffortsConfig, self.callback) + + # Create joint efforts publisher. + self.arm_pub = rospy.Publisher(ARM_TOPIC, Float64MultiArray, queue_size=10) + + # Create gripper width publisher. + self.gripper_move_client = actionlib.SimpleActionClient( + GRIPPER_ACTION_NAME, MoveAction + ) + self.gripper_connected = self.gripper_move_client.wait_for_server( + timeout=rospy.Duration(secs=5) + ) + + def callback(self, config, level): + """Dynamic reconfigure callback function. + + Args: + config (dict): Dictionary containing the new configuration. + level (int): Level of the dynamic reconfigure server. Represents the + variable that was changed or if -1 that the server was just started. + """ + if level == -1: + self._initialize_joint_states(config) + else: + self._log_reconfigure_request(config) + if level < 7: + self._publish_joint_efforts(config) + elif level in [7, 8]: + if self.gripper_connected: + self._send_gripper_command(config) + else: + rospy.logwarn_once( + "Gripper commands not applied since the gripper command " + "action was not found." + ) + return config + + def _initialize_joint_states(self, config): + """Set initial joint states. + + Args: + config (dict): Dictionary containing the new configuration. + """ + rospy.loginfo("Waiting for initial joint states...") + joint_states = rospy.wait_for_message(JOINT_STATES_TOPIC, JointState) + position_dict = dict(zip(joint_states.name, joint_states.position)) + effort_dict = dict(zip(joint_states.name, joint_states.effort)) + set_values = list(effort_dict.values())[:-2] + set_values.append(list(position_dict.values())[-1] * 2) + config.update( + **dict(zip([key for key in config.keys() if key != "groups"], set_values)) + ) + rospy.loginfo("Initial joint states retrieved.") + rospy.loginfo("Dynamic reconfigure server started.") + + def _log_reconfigure_request(self, config): + """Log reconfigure request. + + Args: + config (dict): Dictionary containing the new configuration. + """ + rospy.loginfo( + ( + "Reconfigure Request: {joint1_effort}, {joint2_effort}, " + "{joint3_effort}, {joint4_effort}, {joint5_effort}, {joint6_effort}, " + "{joint7_effort}, {width}, {speed}" + ).format(**config) + ) + + def _publish_joint_efforts(self, config): + """Publish joint efforts. + + Args: + config (dict): Dictionary containing the new configuration. + """ + self.arm_pub.publish(Float64MultiArray(data=list(config.values())[:7])) + + def _send_gripper_command(self, config): + """Send gripper command. + + Args: + config (dict): Dictionary containing the new configuration. + """ + move_goal = MoveGoal(width=config["width"], speed=config["speed"]) + self.gripper_move_client.send_goal(move_goal) + result = self.gripper_move_client.wait_for_result() + if not result: + rospy.logerr("Something went wrong while setting the gripper commands.") + + +if __name__ == "__main__": + rospy.init_node("joint_efforts_reconfig_server", anonymous=False) + + try: + JointEffortsDynamicReconfigureServer() + rospy.spin() + except rospy.ROSInterruptException: + rospy.logerr("ROS node interrupted.") + except Exception as e: + rospy.logerr(f"Unexpected error occurred: {e}") diff --git a/panda_gazebo/scripts/joint_position_dynamic_reconfigure_server.py b/panda_gazebo/scripts/joint_position_dynamic_reconfigure_server.py deleted file mode 100755 index b522c34f..00000000 --- a/panda_gazebo/scripts/joint_position_dynamic_reconfigure_server.py +++ /dev/null @@ -1,117 +0,0 @@ -#!/usr/bin/env python3 -"""Small node that spins up a dynamic reconfigure server that can be used to change the -joint positions. -""" -import actionlib -import rospy -from dynamic_reconfigure.server import Server -from franka_gripper.msg import MoveAction, MoveGoal -from sensor_msgs.msg import JointState -from std_msgs.msg import Float64 - -from panda_gazebo.cfg import JointPositionConfig - - -class JointPositionDynamicReconfigureServer: - """A small node that spins up a dynamic reconfigure server that can be used to - change the joint positions. - """ - - def __init__(self): - """Initialise JointPositionDynamicReconfigureServer object.""" - self.srv = Server(JointPositionConfig, self.callback) - - # Create joint position publishers. - self.arm_joint1_pub = rospy.Publisher( - "panda_arm_joint1_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint2_pub = rospy.Publisher( - "panda_arm_joint2_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint3_pub = rospy.Publisher( - "panda_arm_joint3_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint4_pub = rospy.Publisher( - "panda_arm_joint4_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint5_pub = rospy.Publisher( - "panda_arm_joint5_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint6_pub = rospy.Publisher( - "panda_arm_joint6_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint7_pub = rospy.Publisher( - "panda_arm_joint7_position_controller/command", Float64, queue_size=10 - ) - self.arm_pubs = [ - self.arm_joint1_pub, - self.arm_joint2_pub, - self.arm_joint3_pub, - self.arm_joint4_pub, - self.arm_joint5_pub, - self.arm_joint6_pub, - self.arm_joint7_pub, - ] - self.gripper_move_client = actionlib.SimpleActionClient( - "franka_gripper/move", MoveAction - ) - self.gripper_connected = self.gripper_move_client.wait_for_server( - timeout=rospy.Duration(secs=5) - ) - - def callback(self, config, level): - """Dynamic reconfigure callback function. - - Args: - config (:obj:`dynamic_reconfigure.encoding.Config`): The current dynamic - reconfigure configuration object. - level (int): Bitmask that gives information about which parameter has been - changed. - - Returns: - :obj:`~dynamic_reconfigure.encoding.Config`: Modified dynamic reconfigure - configuration object. - """ - rospy.loginfo( - ( - "Reconfigure Request: {joint1_position}, {joint2_position}, " - "{joint3_position} {joint4_position}, {joint5_position}, " - "{joint6_position}, {joint7_position}, {width}, {speed}" - ).format(**config) - ) - - # Set initial joint states. - if level == -1: - joint_states = rospy.wait_for_message("joint_states", JointState) - position_dict = dict(zip(joint_states.name, joint_states.position)) - set_values = list(position_dict.values())[:-2] - set_values.append(list(position_dict.values())[-1] * 2) - config.update( - **dict( - zip([key for key in config.keys() if key != "groups"], set_values) - ) - ) - - # Write joint positions to controller topics. - if level > -1 and level < 7: - self.arm_pubs[level].publish(list(config.values())[level]) - elif level in [7, 8] and self.gripper_connected: - move_goal = MoveGoal(width=config["width"], speed=config["speed"]) - self.gripper_move_client.send_goal(move_goal) - result = self.gripper_move_client.wait_for_result() - if not result: - rospy.logerr("Something went wrong while setting the gripper commands.") - elif level in [7, 8] and not self.gripper_connected: - rospy.logwarn_once( - "Gripper commands not applied since the gripper command action was not " - "found." - ) - return config - - -if __name__ == "__main__": - rospy.init_node("joint_position_reconfig_server", anonymous=False) - - position_dyn_server = JointPositionDynamicReconfigureServer() - - rospy.spin() diff --git a/panda_gazebo/scripts/joint_positions_dynamic_reconfigure_server.py b/panda_gazebo/scripts/joint_positions_dynamic_reconfigure_server.py new file mode 100755 index 00000000..51261196 --- /dev/null +++ b/panda_gazebo/scripts/joint_positions_dynamic_reconfigure_server.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +"""Small node that spins up a dynamic reconfigure server that can be used to change the +panda arm joint positions and gripper width. +""" +import actionlib +import rospy +from dynamic_reconfigure.server import Server +from franka_gripper.msg import MoveAction, MoveGoal +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64MultiArray + +from panda_gazebo.cfg import JointPositionsConfig + +# Constants for topic names +ARM_TOPIC = "/panda_arm_joint_position_controller/command" +JOINT_STATES_TOPIC = "joint_states" +GRIPPER_ACTION_NAME = "franka_gripper/move" + + +class JointPositionsDynamicReconfigureServer: + """A small node that spins up a dynamic reconfigure server that can be used to + change the panda arm joint positions and gripper width. + """ + + def __init__(self): + """Initialise JointPositionsDynamicReconfigureServer object.""" + rospy.loginfo("Starting dynamic reconfigure server...") + self.srv = Server(JointPositionsConfig, self.callback) + + # Create joint positions publisher. + self.arm_pub = rospy.Publisher(ARM_TOPIC, Float64MultiArray, queue_size=10) + + # Create gripper width publisher. + self.gripper_move_client = actionlib.SimpleActionClient( + GRIPPER_ACTION_NAME, MoveAction + ) + self.gripper_connected = self.gripper_move_client.wait_for_server( + timeout=rospy.Duration(secs=5) + ) + + def callback(self, config, level): + """Dynamic reconfigure callback function. + + Args: + config (dict): Dictionary containing the new configuration. + level (int): Level of the dynamic reconfigure server. Represents the + variable that was changed or if -1 that the server was just started. + """ + if level == -1: + self._initialize_joint_states(config) + else: + self._log_reconfigure_request(config) + if level < 7: + self._publish_joint_positions(config) + elif level in [7, 8]: + if self.gripper_connected: + self._send_gripper_command(config) + else: + rospy.logwarn_once( + "Gripper commands not applied since the gripper command " + "action was not found." + ) + return config + + def _initialize_joint_states(self, config): + """Set initial joint states. + + Args: + config (dict): Dictionary containing the new configuration. + """ + rospy.loginfo("Waiting for initial joint states...") + joint_states = rospy.wait_for_message(JOINT_STATES_TOPIC, JointState) + position_dict = dict(zip(joint_states.name, joint_states.position)) + set_values = list(position_dict.values()) + config.update( + **dict(zip([key for key in config.keys() if key != "groups"], set_values)) + ) + rospy.loginfo("Initial joint states retrieved.") + rospy.loginfo("Dynamic reconfigure server started.") + + def _log_reconfigure_request(self, config): + """Log reconfigure request. + + Args: + config (dict): Dictionary containing the new configuration. + """ + rospy.loginfo( + ( + "Reconfigure Request: {joint1_position}, {joint2_position}, " + "{joint3_position}, {joint4_position}, {joint5_position}, " + "{joint6_position}, {joint7_position}, {width}, {speed}" + ).format(**config) + ) + + def _publish_joint_positions(self, config): + """Publish joint positions. + + Args: + config (dict): Dictionary containing the new configuration. + """ + self.arm_pub.publish(Float64MultiArray(data=list(config.values())[:7])) + + def _send_gripper_command(self, config): + """Send gripper command. + + Args: + config (dict): Dictionary containing the new configuration. + """ + move_goal = MoveGoal(width=config["width"], speed=config["speed"]) + self.gripper_move_client.send_goal(move_goal) + result = self.gripper_move_client.wait_for_result() + if not result: + rospy.logerr("Something went wrong while setting the gripper commands.") + + +if __name__ == "__main__": + rospy.init_node("joint_positions_reconfig_server", anonymous=False) + + try: + JointPositionsDynamicReconfigureServer() + rospy.spin() + except rospy.ROSInterruptException: + rospy.logerr("ROS node interrupted.") + except Exception as e: + rospy.logerr(f"Unexpected error occurred: {e}") diff --git a/panda_gazebo/setup.cfg b/panda_gazebo/setup.cfg index 707c21c2..d4361686 100755 --- a/panda_gazebo/setup.cfg +++ b/panda_gazebo/setup.cfg @@ -6,6 +6,8 @@ exclude = tests, conf.py, node_modules + setup.py per-file-ignores = __init__.py: F401, E501 max-complexity = 10 +extend-ignore = E203, W503, D400, D401, D205 diff --git a/panda_gazebo/src/panda_gazebo/__init__.py b/panda_gazebo/src/panda_gazebo/__init__.py index 42937f76..9eedae75 100644 --- a/panda_gazebo/src/panda_gazebo/__init__.py +++ b/panda_gazebo/src/panda_gazebo/__init__.py @@ -1,3 +1,3 @@ -# Make module version available. +"""Make module version available.""" from .version import __version__ # noqa: F401 from .version import __version_tuple__ # noqa: F401 diff --git a/panda_gazebo/src/panda_gazebo/common/controlled_joints_dict.py b/panda_gazebo/src/panda_gazebo/common/controlled_joints_dict.py index 06280a92..7d8304a7 100644 --- a/panda_gazebo/src/panda_gazebo/common/controlled_joints_dict.py +++ b/panda_gazebo/src/panda_gazebo/common/controlled_joints_dict.py @@ -1,5 +1,4 @@ -"""Class used to store information about the currently controlled joints. -""" +"""Class used to store information about the currently controlled joints.""" import copy CONTROLLED_JOINTS_DICT = { @@ -16,6 +15,6 @@ class ControlledJointsDict(dict): """ def __init__(self, *args, **kwargs): - """Initialise the ControllerInfoDict""" + """Initialise the ControllerInfoDict.""" super().__init__(*args, **kwargs) super().update(copy.deepcopy(CONTROLLED_JOINTS_DICT)) diff --git a/panda_gazebo/src/panda_gazebo/common/controller_info_dict.py b/panda_gazebo/src/panda_gazebo/common/controller_info_dict.py index 23f9fd0d..7bd6d592 100644 --- a/panda_gazebo/src/panda_gazebo/common/controller_info_dict.py +++ b/panda_gazebo/src/panda_gazebo/common/controller_info_dict.py @@ -1,5 +1,4 @@ -"""Class used to store information about the Gazebo controllers. -""" +"""Class used to store information about the Gazebo controllers.""" import copy CONTROLLER_INFO_DICT = { @@ -73,6 +72,6 @@ class ControllerInfoDict(dict): """ def __init__(self, *args, **kwargs): - """Initialise the ControllerInfoDict""" + """Initialise the ControllerInfoDict.""" super().__init__(*args, **kwargs) super().update(copy.deepcopy(CONTROLLER_INFO_DICT)) diff --git a/panda_gazebo/src/panda_gazebo/core/__init__.py b/panda_gazebo/src/panda_gazebo/core/__init__.py index c33fdf86..672cd7df 100644 --- a/panda_gazebo/src/panda_gazebo/core/__init__.py +++ b/panda_gazebo/src/panda_gazebo/core/__init__.py @@ -3,5 +3,4 @@ """ from panda_gazebo.core.control_server import PandaControlServer from panda_gazebo.core.control_switcher import PandaControlSwitcher -from panda_gazebo.core.group_publisher import GroupPublisher from panda_gazebo.core.moveit_server import PandaMoveItPlannerServer diff --git a/panda_gazebo/src/panda_gazebo/core/control_server.py b/panda_gazebo/src/panda_gazebo/core/control_server.py index 6d32f332..7bce8951 100644 --- a/panda_gazebo/src/panda_gazebo/core/control_server.py +++ b/panda_gazebo/src/panda_gazebo/core/control_server.py @@ -37,7 +37,7 @@ from controller_manager_msgs.srv import ListControllers, ListControllersRequest from rospy.exceptions import ROSException, ROSInterruptException from sensor_msgs.msg import JointState -from std_msgs.msg import Float64 +from std_msgs.msg import Float64MultiArray from panda_gazebo.common import ControlledJointsDict from panda_gazebo.common.helpers import ( @@ -51,7 +51,6 @@ ros_exit_gracefully, translate_actionclient_result_error_code, ) -from panda_gazebo.core.group_publisher import GroupPublisher from panda_gazebo.exceptions import InputMessageInvalidError from panda_gazebo.msg import FollowJointTrajectoryAction, FollowJointTrajectoryResult from panda_gazebo.srv import ( @@ -86,26 +85,10 @@ ], "hand": ["panda_finger_joint1", "panda_finger_joint2"], } -ARM_POSITION_CONTROLLERS = [ - "panda_arm_joint1_position_controller", - "panda_arm_joint2_position_controller", - "panda_arm_joint3_position_controller", - "panda_arm_joint4_position_controller", - "panda_arm_joint5_position_controller", - "panda_arm_joint6_position_controller", - "panda_arm_joint7_position_controller", -] -ARM_EFFORT_CONTROLLERS = [ - "panda_arm_joint1_effort_controller", - "panda_arm_joint2_effort_controller", - "panda_arm_joint3_effort_controller", - "panda_arm_joint4_effort_controller", - "panda_arm_joint5_effort_controller", - "panda_arm_joint6_effort_controller", - "panda_arm_joint7_effort_controller", -] -ARM_TRAJ_CONTROLLERS = ["panda_arm_controller"] -HAND_CONTROLLERS = ["franka_gripper"] +ARM_POSITION_CONTROLLER = "panda_arm_joint_position_controller" +ARM_EFFORT_CONTROLLER = "panda_arm_joint_effort_controller" +ARM_TRAJ_CONTROLLER = "panda_arm_controller" +HAND_CONTROLLER = "franka_gripper" CONTROLLER_INFO_RATE = 1 / 10 # Rate [hz] for retrieving controller information. CONNECTION_TIMEOUT = 10 # Service connection timeout [s]. @@ -247,27 +230,17 @@ def __init__( # noqa: C901 # services and messages. ############### ######################################## if load_set_joint_commands_service: - # Create arm joint position controller publishers. - self._arm_joint_position_pub = GroupPublisher() - for position_controller in ARM_POSITION_CONTROLLERS: - self._arm_joint_position_pub.append( - rospy.Publisher( - "%s/command" % (position_controller), - Float64, - queue_size=10, - ) - ) + # Create arm joint position controller publisher. + self._arm_joint_position_pub = rospy.Publisher( + "%s/command" % (ARM_POSITION_CONTROLLER), + Float64MultiArray, + queue_size=10, + ) - # Create arm joint effort publishers. - self._arm_joint_effort_pub = GroupPublisher() - for effort_controller in ARM_EFFORT_CONTROLLERS: - self._arm_joint_effort_pub.append( - rospy.Publisher( - "%s/command" % (effort_controller), - Float64, - queue_size=10, - ) - ) + # Create arm joint effort publisher. + self._arm_joint_effort_pub = rospy.Publisher( + "%s/command" % (ARM_EFFORT_CONTROLLER), Float64MultiArray, queue_size=10 + ) # Connect to controller_manager services. try: @@ -452,12 +425,12 @@ def _wait_till_arm_control_done( rospy.logwarn( "Not waiting for control to be completed as no joints appear to be " "controlled when using '%s' control. Please make sure the '%s' " - "controllers that are needed for '%s' control are initialised." + "controller that is needed for '%s' control is initialised." % ( control_type, - ARM_POSITION_CONTROLLERS + ARM_POSITION_CONTROLLER if control_type == "position" - else ARM_EFFORT_CONTROLLERS, + else ARM_EFFORT_CONTROLLER, control_type, ) ) @@ -520,6 +493,11 @@ def _create_arm_traj_action_server_msg(self, input_msg): # noqa: C901 `follow_joint_trajectory `_ action server. + By doing this, we allow our users to send incomplete joint trajectory messages + (i.e., joint trajectory messages that do not contain all the joints). We also + allow them to generate the time axes automatically when the ``create_time_axis`` + field is set to ``True``. + Args: input_msg (:obj:`trajectory_msgs/JointTrajectory`): The service input message we want to validate. @@ -956,8 +934,8 @@ def _create_arm_traj_action_server_msg(self, input_msg): # noqa: C901 return panda_action_msg_2_control_msgs_action_msg(arm_control_msg) - def _create_control_publisher_msg(self, input_msg, control_type): # noqa: C901 - """Converts the input message of the position/effort control service into a + def _create_arm_control_publisher_msg(self, input_msg, control_type): # noqa: C901 + """Converts the input message of the arm position/effort control service into a control commands that is used by the control publishers. While doing this it also verifies whether the given input message is valid. @@ -976,41 +954,52 @@ def _create_control_publisher_msg(self, input_msg, control_type): # noqa: C901 input_msg could not be converted into arm joint position/effort commands. """ # noqa: E501 - if control_type not in ["position", "effort"]: + valid_control_types = ["position", "effort"] + if control_type not in valid_control_types: raise ValueError( - "Please specify a valid control type. Valid values are 'position' & " - "'effort'." + "Invalid control type. Valid values are 'position' & 'effort'." ) - else: - control_type = control_type.lower() - if control_type == "position": - input_joint_names = input_msg.joint_names - control_input = input_msg.joint_positions - elif control_type == "effort": - input_joint_names = input_msg.joint_names - control_input = input_msg.joint_efforts - # Validate and handle service request input. + # Parse control input. + control_type = control_type.lower() + input_joint_names = input_msg.joint_names + control_input = ( + input_msg.joint_positions + if control_type == "position" + else input_msg.joint_efforts + ) + + # Retrieve controlled joints and current joint state. controlled_joints = self.controlled_joints[control_type]["arm"] controlled_joints_size = len(controlled_joints) - if len(input_joint_names) == 0: # If not joint_names were given. + joint_values = ( + self.joint_states.position + if control_type == "position" + else self.joint_states.effort + ) + arm_commands_dict = { + key: val + for key, val in zip(self.joint_states.name, joint_values) + if key in controlled_joints + } + + # Handle the case when no joint_names were given. + if len(input_joint_names) == 0: # Check if enough joint position commands were given otherwise give warning. if len(control_input) != controlled_joints_size: - logwarn_msg_strings = [ + joint_str = ( f"joint {control_type}" if len(control_input) == 1 - else f"joint {control_type}", - "joint" if controlled_joints_size == 1 else "joints", - ] + else f"joint {control_type}s" + ) + joint_names_str = "joint" if controlled_joints_size == 1 else "joints" # Check if control input is bigger than controllable joints. if len(control_input) > controlled_joints_size: logwarn_message = ( - "You specified %s while the Panda arm control group has %s." - % ( - "%s %s" % (len(control_input), logwarn_msg_strings[0]), - "%s %s" % (controlled_joints_size, logwarn_msg_strings[1]), - ) + f"You specified {len(control_input)} {joint_str} while the " + f"Panda arm control group has {controlled_joints_size} " + f"{joint_names_str}." ) raise InputMessageInvalidError( message=f"Invalid number of joint {control_type} commands.", @@ -1022,115 +1011,81 @@ def _create_control_publisher_msg(self, input_msg, control_type): # noqa: C901 ) elif len(control_input) < controlled_joints_size: logwarn_message = ( - "You specified %s while the Panda arm control group has %s." - % ( - "%s %s" % (len(control_input), logwarn_msg_strings[0]), - "%s %s" % (controlled_joints_size, logwarn_msg_strings[1]), - ) - + " As a result only joints %s will be controlled." - % (controlled_joints[: len(control_input)]) + f"You specified {len(control_input)} {joint_str} while the " + f"Panda arm control group has {controlled_joints_size} " + f"{joint_names_str}. As a result only joints " + f"{controlled_joints[: len(control_input)]} will be controlled." ) rospy.logwarn(logwarn_message) - # Update current state dict with given joint_position commands. - arm_position_commands = [ - val - for key, val in dict( - zip(self.joint_states.name, self.joint_states.position) - ).items() - if key in controlled_joints + # Add control commands to arm command dict. + joints_to_update = list(arm_commands_dict.keys())[ + : len(control_input) ] - arm_position_commands[: len(control_input)] = control_input - - # Return command message. - control_commands = [Float64(item) for item in arm_position_commands] - return control_commands + arm_commands_dict = { + k: v + if k not in joints_to_update + else control_input[joints_to_update.index(k)] + for k, v in arm_commands_dict.items() + } else: - # Check if enough control values were given. + # Throw warning if not enough control values were given. if len(input_joint_names) != len(control_input): - logwarn_msg_strings = [ + joint_str = ( f"joint {control_type}" if len(control_input) == 1 - else f"joint {control_type}s", - f"panda_gazebo/SetJoint{control_type.capitalize()}s", - "joint" if len(input_joint_names) == 1 else "joints", - f"joint {control_type}", - f"joint_{control_type}s", - ] + else f"joint {control_type}s" + ) + joint_names_str = "joint" if len(input_joint_names) == 1 else "joints" logwarn_message = ( - "You specified %s while the 'joint_names' field of the " - "'%s' message contains %s. Please make sure you supply " - "a %s for each of the joints contained in the 'joint_names' " - "field." - % ( - "%s %s" % (len(control_input), logwarn_msg_strings[0]), - logwarn_msg_strings[1], - "%s %s" % (len(input_joint_names), logwarn_msg_strings[2]), - logwarn_msg_strings[3], - ) + f"You specified {len(control_input)} {joint_str} while the " + "'joint_names' field of the " + f"'panda_gazebo/SetJoint{control_type.capitalize()}s' message " + f"contains {len(input_joint_names)} {joint_names_str}. " + f"Please make sure you supply a {joint_str} for each of the joints " + "contained in the 'joint_names' field." ) raise InputMessageInvalidError( message=( - f"The 'joint_names' and '{logwarn_msg_strings[4]}' fields " - "of the input message are of different lengths." + f"The 'joint_names' and 'joint_{control_type}s' fields of the " + "input message are of different lengths." ), log_message=logwarn_message, details={ - logwarn_msg_strings[4] + "_length": len(control_input), + f"joint_{control_type}s_length": len(control_input), "joint_names_length": len(input_joint_names), }, ) - else: - # Validate joint_names. - invalid_joint_names = [ - joint_name - for joint_name in input_joint_names - if joint_name not in controlled_joints - ] - if len(invalid_joint_names) != 0: # Joint names invalid. - logwarn_msg_strings = [ - "Joint" if len(invalid_joint_names) == 1 else "Joints", - "was" if len(invalid_joint_names) == 1 else "were", - f"panda_gazebo/SetJoint{control_type.capitalize()}", - ] - logwarn_message = ( - "%s that %s specified in the 'joint_names' field of the '%s' " - "message %s invalid. Valid joint names for controlling the " - "Panda arm are %s." - % ( - "%s %s" % (logwarn_msg_strings[0], invalid_joint_names), - logwarn_msg_strings[1], - logwarn_msg_strings[2], - logwarn_msg_strings[1], - controlled_joints, - ) - ) - raise InputMessageInvalidError( - message="Invalid joint_names were given.", - log_message=logwarn_message, - details={"invalid_joint_names": invalid_joint_names}, - ) - else: - # Update current state dict with given joint_position commands. - arm_position_commands_dict = { - key: val - for key, val in dict( - zip(self.joint_states.name, self.joint_states.position) - ).items() - if key in controlled_joints - } - for ( - joint, - position, - ) in dict(zip(input_joint_names, control_input)).items(): - if joint in arm_position_commands_dict.keys(): - arm_position_commands_dict[joint] = position - control_commands = [ - Float64(item) for item in arm_position_commands_dict.values() - ] - # Return command message. - return control_commands + # Throw error if joint_names are invalid. + invalid_joint_names = [ + joint_name + for joint_name in input_joint_names + if joint_name not in controlled_joints + ] + if invalid_joint_names: # Joint names invalid. + joint_str = "Joint" if len(invalid_joint_names) == 1 else "Joints" + was_were = "was" if len(invalid_joint_names) == 1 else "were" + logwarn_message = ( + f"{joint_str} {invalid_joint_names} that {was_were} specified in " + "the 'joint_names' field of the " + f"'panda_gazebo/SetJoint{control_type.capitalize()}' message " + f"{was_were} invalid. Valid joint names for controlling the Panda " + f"arm are {controlled_joints}." + ) + raise InputMessageInvalidError( + message="Invalid joint_names were given.", + log_message=logwarn_message, + details={"invalid_joint_names": invalid_joint_names}, + ) + + # Add control commands to arm command dict. + for joint, command in zip(input_joint_names, control_input): + if joint in arm_commands_dict: + arm_commands_dict[joint] = command + + # Return command message. + return Float64MultiArray(data=list(arm_commands_dict.values())) def _split_joint_commands_req(self, joint_commands_req, control_type): """Splits the combined :obj:`~panda_gazebo.msg.SetJointControlCommandRequest` @@ -1281,43 +1236,43 @@ def controlled_joints(self): # noqa: C901 if not self.__controlled_joints: controllers = self.controllers controlled_joints_dict = ControlledJointsDict() - for controller in ARM_POSITION_CONTROLLERS + HAND_CONTROLLERS: + for controller in [ARM_POSITION_CONTROLLER, HAND_CONTROLLER]: try: for claimed_resources in controllers[controller].claimed_resources: for resource in claimed_resources.resources: - if controller in ARM_POSITION_CONTROLLERS: + if controller == ARM_POSITION_CONTROLLER: controlled_joints_dict["position"]["arm"].append( resource ) - elif controller in HAND_CONTROLLERS: + elif controller == HAND_CONTROLLER: controlled_joints_dict["position"]["hand"].append( resource ) controlled_joints_dict["position"]["both"].append(resource) except KeyError: # Controller not initialised. pass - for controller in ARM_EFFORT_CONTROLLERS + HAND_CONTROLLERS: + for controller in [ARM_EFFORT_CONTROLLER, HAND_CONTROLLER]: try: for claimed_resources in controllers[controller].claimed_resources: for resource in claimed_resources.resources: - if controller in ARM_EFFORT_CONTROLLERS: + if controller == ARM_EFFORT_CONTROLLER: controlled_joints_dict["effort"]["arm"].append(resource) - elif controller in HAND_CONTROLLERS: + elif controller == HAND_CONTROLLER: controlled_joints_dict["effort"]["hand"].append( resource ) controlled_joints_dict["effort"]["both"].append(resource) except KeyError: # Controller not initialised. pass - for controller in ARM_TRAJ_CONTROLLERS + HAND_CONTROLLERS: + for controller in [ARM_TRAJ_CONTROLLER, HAND_CONTROLLER]: try: for claimed_resources in controllers[controller].claimed_resources: for resource in claimed_resources.resources: - if controller in ARM_TRAJ_CONTROLLERS: + if controller == ARM_TRAJ_CONTROLLER: controlled_joints_dict["trajectory"]["arm"].append( resource ) - elif controller in HAND_CONTROLLERS: + elif controller == HAND_CONTROLLER: controlled_joints_dict["trajectory"]["hand"].append( resource ) @@ -1424,15 +1379,19 @@ def _set_joint_commands_cb(self, set_joint_commands_req): set_joint_commands_req, control_type ) - # Send control commands. - gripper_result = False + # Send arm control commands. + arm_resp = None if arm_command_msg is not None: - if control_type == "position": - arm_resp = self._arm_set_joint_positions_cb(arm_command_msg) - else: - arm_resp = self._arm_set_joint_efforts_cb(arm_command_msg) + arm_resp = ( + self._arm_set_joint_positions_cb(arm_command_msg) + if control_type == "position" + else self._arm_set_joint_efforts_cb(arm_command_msg) + ) + + # Send gripper control commands. + gripper_resp = None if self._load_gripper and gripper_command_msg is not None: - gripper_result = self._set_gripper_width_cb(gripper_command_msg) + gripper_resp = self._set_gripper_width_cb(gripper_command_msg) elif gripper_command_msg is not None: rospy.logwarn_once( f"Gripper command could not be set since the '{rospy.get_name()}' " @@ -1441,26 +1400,25 @@ def _set_joint_commands_cb(self, set_joint_commands_req): f"'{rospy.get_name()}'." ) - # Return result. - resp = SetJointCommandsResponse() + # Check if everything went OK and return response. if ( - (self._load_gripper and all([gripper_result, arm_resp.success])) - or not self._load_gripper + arm_resp is not None and arm_resp.success + and (not self._load_gripper or gripper_resp) ): resp.success = True resp.message = "Everything went OK" - elif self._load_gripper and all( - [not item for item in [gripper_result, arm_resp.success]] + elif ( + self._load_gripper and gripper_command_msg is not None and not gripper_resp ): resp.success = False - resp.message = "Joint control failed" - elif not arm_resp.success: + resp.message = "Gripper control failed" + elif arm_resp is not None and not arm_resp.success: resp.success = False resp.message = "Arm control failed" - elif self._load_gripper and not gripper_result: + elif self._load_gripper and not gripper_resp: resp.success = False - resp.message = "Gripper control failed" + resp.message = "Joint control failed" return resp def _arm_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 @@ -1491,7 +1449,7 @@ def _arm_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 # NOTE: Fail if missing and display warning if not started. resp = SetJointPositionsResponse() missing_controllers, stopped_controllers = self._controllers_running( - ARM_POSITION_CONTROLLERS + [ARM_POSITION_CONTROLLER] ) if len(missing_controllers) >= 1: rospy.logwarn( @@ -1538,7 +1496,7 @@ def _arm_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 # Validate request and publish control command. try: - control_pub_msgs = self._create_control_publisher_msg( + control_pub_msgs = self._create_arm_control_publisher_msg( input_msg=set_joint_positions_req, control_type="position", ) @@ -1557,7 +1515,7 @@ def _arm_set_joint_positions_cb(self, set_joint_positions_req): # noqa: C901 if set_joint_positions_req.wait and not len(stopped_controllers) >= 1: self._wait_till_arm_control_done( control_type="position", - joint_setpoint=[command.data for command in control_pub_msgs], + joint_setpoint=control_pub_msgs.data, ) resp.success = True @@ -1591,7 +1549,7 @@ def _arm_set_joint_efforts_cb(self, set_joint_efforts_req): # NOTE: Fail if is missing and display warning if not started. resp = SetJointEffortsResponse() missing_controllers, stopped_controllers = self._controllers_running( - ARM_EFFORT_CONTROLLERS + [ARM_EFFORT_CONTROLLER] ) if len(missing_controllers) >= 1: rospy.logwarn( @@ -1638,7 +1596,7 @@ def _arm_set_joint_efforts_cb(self, set_joint_efforts_req): # Validate request and publish control command. try: - control_pub_msgs = self._create_control_publisher_msg( + control_pub_msgs = self._create_arm_control_publisher_msg( input_msg=set_joint_efforts_req, control_type="effort", ) @@ -1707,10 +1665,10 @@ def _set_gripper_width_cb(self, set_gripper_width_req): # Wait for result. if set_gripper_width_req.wait: - gripper_result = self._gripper_command_client.wait_for_result( + gripper_resp = self._gripper_command_client.wait_for_result( timeout=rospy.Duration.from_sec(WAIT_TILL_DONE_TIMEOUT) ) - resp.success = gripper_result + resp.success = gripper_resp else: resp.success = True else: diff --git a/panda_gazebo/src/panda_gazebo/core/control_switcher.py b/panda_gazebo/src/panda_gazebo/core/control_switcher.py index 0d0199f8..a0dbdab3 100644 --- a/panda_gazebo/src/panda_gazebo/core/control_switcher.py +++ b/panda_gazebo/src/panda_gazebo/core/control_switcher.py @@ -1,5 +1,5 @@ """This class is responsible for switching the control type that is used for -controlling the Panda Robot robot ``arm``. It serves as a wrapper aroundthe services +controlling the Panda Robot robot ``arm``. It serves as a wrapper around the services created by the ROS `controller_manager `_ class. Control types: diff --git a/panda_gazebo/src/panda_gazebo/core/group_publisher.py b/panda_gazebo/src/panda_gazebo/core/group_publisher.py deleted file mode 100644 index 09960a86..00000000 --- a/panda_gazebo/src/panda_gazebo/core/group_publisher.py +++ /dev/null @@ -1,70 +0,0 @@ -"""Class used to group a number of publishers together. -""" -import rospy - - -class GroupPublisher(list): - """Used for bundling ROS publishers together and publishing - to these publishers at the same time. - """ - - def __init__(self, iterable=[]): - """Initialise group publisher object. - - Args: - iterable (list, optional): New list initialised from iterable items. - Defaults to ``[]``. - - Raises: - ValueError: If list does not only contain ROS publishers. - """ - # Validate if list contains publisher objects. - if type(iterable) is list: - for publisher in iterable: - if type(publisher) is not rospy.Publisher: - raise ValueError( - "Please supply a list containing only ros publishers." - ) - else: - if type(iterable) is not rospy.Publisher: - raise ValueError("Please supply a list containing only ros publishers.") - - super(GroupPublisher, self).__init__(iterable) - - def publish(self, messages): - """Publishes a list of messages to the publishers contained on the - GroupPublisher object. The index of the message corresponds to the - publisher the message will be published to. - - Args: - messages (list): List containing the messages to be published. - - Raises: - ValueError: If something went wrong during publishing. - """ - # Validate input messages. - if self.__len__() == 0: - raise ValueError( - "Message could not be published since GroupPublisher " - "contains no publishers." - ) - elif self.__len__() > 1 and type(messages) is not list: - raise ValueError( - "Only one message was given while the GroupPublisher object " - "contains %s publishers. Please input a list containing %s ROS " - "messages." % (self.__len__(), self.__len__()) - ) - elif self.__len__() > 1 and type(messages) is list: - if self.__len__() != len(messages): - raise ValueError( - "%s messages were given while the GroupPublisher object " - "contains %s publishers. Please input a list containing %s ROS " - "messages." % (len(messages), self.__len__(), self.__len__()) - ) - - # Publish messages to the publishers. - if type(messages) is not list: - self[0].publish(messages) - else: - for ii in range(len(messages)): - self[ii].publish(messages[ii]) diff --git a/panda_gazebo/src/panda_gazebo/version.py b/panda_gazebo/src/panda_gazebo/version.py index 1addf8f4..21ad8877 100644 --- a/panda_gazebo/src/panda_gazebo/version.py +++ b/panda_gazebo/src/panda_gazebo/version.py @@ -1,3 +1,3 @@ -# Stores the package version number so that it can be accessed from other modules. +"""Stores the package version number so that it can be accessed from other modules.""" __version__ = "2.15.4" __version_tuple__ = __version__.split(".") diff --git a/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server.py b/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server.py deleted file mode 100755 index 6db474e3..00000000 --- a/panda_gazebo/tests/manual/joint_effort_dynamic_reconfigure_server.py +++ /dev/null @@ -1,113 +0,0 @@ -#!/usr/bin/env python3 -"""Small node that spins up a dynamic reconfigure server that can be used to change the -joint efforts. -""" -import actionlib -import rospy -from dynamic_reconfigure.server import Server -from franka_gripper.msg import MoveAction, MoveGoal -from sensor_msgs.msg import JointState -from std_msgs.msg import Float64 - -from panda_gazebo.cfg import JointEffortConfig - - -class JointEffortDynamicReconfigureServer: - def __init__(self): - self.srv = Server(JointEffortConfig, self.callback) - - # Create joint effort publishers. - self.arm_joint1_pub = rospy.Publisher( - "panda_arm_joint1_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint2_pub = rospy.Publisher( - "panda_arm_joint2_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint3_pub = rospy.Publisher( - "panda_arm_joint3_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint4_pub = rospy.Publisher( - "panda_arm_joint4_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint5_pub = rospy.Publisher( - "panda_arm_joint5_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint6_pub = rospy.Publisher( - "panda_arm_joint6_effort_controller/command", Float64, queue_size=10 - ) - self.arm_joint7_pub = rospy.Publisher( - "panda_arm_joint7_effort_controller/command", Float64, queue_size=10 - ) - self.arm_pubs = [ - self.arm_joint1_pub, - self.arm_joint2_pub, - self.arm_joint3_pub, - self.arm_joint4_pub, - self.arm_joint5_pub, - self.arm_joint6_pub, - self.arm_joint7_pub, - ] - self.gripper_move_client = actionlib.SimpleActionClient( - "franka_gripper/move", MoveAction - ) - self.gripper_connected = self.gripper_move_client.wait_for_server( - timeout=rospy.Duration(secs=5) - ) - - def callback(self, config, level): - """Dynamic reconfigure callback function. - - Args: - config (:obj:`dynamic_reconfigure.encoding.Config`): The current dynamic - reconfigure configuration object. - level (int): Bitmask that gives information about which parameter has been - changed. - - Returns: - :obj:`~dynamic_reconfigure.encoding.Config`: Modified dynamic reconfigure - configuration object. - """ - rospy.loginfo( - ( - "Reconfigure Request: {joint1_effort}, {joint2_effort}, " - "{joint3_effort} {joint4_effort}, {joint5_effort}, {joint6_effort}, " - "{joint7_effort}, {width}, {speed}" - ).format(**config) - ) - - # Set initial joint states. - if level == -1: - joint_states = rospy.wait_for_message("joint_states", JointState) - position_dict = dict(zip(joint_states.name, joint_states.position)) - effort_dict = dict(zip(joint_states.name, joint_states.effort)) - set_values = list(effort_dict.values())[:-2] - set_values.append(list(position_dict.values())[-1] * 2) - config.update( - **dict( - zip([key for key in config.keys() if key != "groups"], set_values) - ) - ) - - # Write joint positions to controller topics. - if level > -1 and level < 7: - self.arm_pubs[level].publish(list(config.values())[level]) - elif level in [7, 8] and self.gripper_connected: - move_goal = MoveGoal(width=config["width"], speed=config["speed"]) - self.gripper_move_client.send_goal(move_goal) - result = self.gripper_move_client.wait_for_result() - if not result: - rospy.logerr("Something went wrong while setting the gripper commands.") - elif level in [7, 8] and not self.gripper_connected: - rospy.logwarn_once( - "Gripper commands not applied since the gripper command action was not " - "found." - ) - return config - - -if __name__ == "__main__": - rospy.init_node("joint_effort_reconfig_server", anonymous=False) - - effort_dyn_server = JointEffortDynamicReconfigureServer() - - rospy.spin() diff --git a/panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server.py b/panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server.py deleted file mode 100755 index 87309baf..00000000 --- a/panda_gazebo/tests/manual/joint_position_dynamic_reconfigure_server.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 -"""Small node that spins up a dynamic reconfigure server that can be used to change the -joint positions. -""" -import actionlib -import rospy -from dynamic_reconfigure.server import Server -from franka_gripper.msg import MoveAction, MoveGoal -from sensor_msgs.msg import JointState -from std_msgs.msg import Float64 - -from panda_gazebo.cfg import JointPositionConfig - - -class JointPositionDynamicReconfigureServer: - def __init__(self): - self.srv = Server(JointPositionConfig, self.callback) - - # Create joint position publishers. - self.arm_joint1_pub = rospy.Publisher( - "panda_arm_joint1_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint2_pub = rospy.Publisher( - "panda_arm_joint2_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint3_pub = rospy.Publisher( - "panda_arm_joint3_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint4_pub = rospy.Publisher( - "panda_arm_joint4_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint5_pub = rospy.Publisher( - "panda_arm_joint5_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint6_pub = rospy.Publisher( - "panda_arm_joint6_position_controller/command", Float64, queue_size=10 - ) - self.arm_joint7_pub = rospy.Publisher( - "panda_arm_joint7_position_controller/command", Float64, queue_size=10 - ) - self.arm_pubs = [ - self.arm_joint1_pub, - self.arm_joint2_pub, - self.arm_joint3_pub, - self.arm_joint4_pub, - self.arm_joint5_pub, - self.arm_joint6_pub, - self.arm_joint7_pub, - ] - self.gripper_move_client = actionlib.SimpleActionClient( - "franka_gripper/move", MoveAction - ) - self.gripper_connected = self.gripper_move_client.wait_for_server( - timeout=rospy.Duration(secs=5) - ) - - def callback(self, config, level): - """Dynamic reconfigure callback function. - - Args: - config (:obj:`dynamic_reconfigure.encoding.Config`): The current dynamic - reconfigure configuration object. - level (int): Bitmask that gives information about which parameter has been - changed. - - Returns: - :obj:`~dynamic_reconfigure.encoding.Config`: Modified dynamic reconfigure - configuration object. - """ - rospy.loginfo( - ( - "Reconfigure Request: {joint1_position}, {joint2_position}, " - "{joint3_position} {joint4_position}, {joint5_position}, " - "{joint6_position}, {joint7_position}, {width}, {speed}" - ).format(**config) - ) - - # Set initial joint states. - if level == -1: - joint_states = rospy.wait_for_message("joint_states", JointState) - position_dict = dict(zip(joint_states.name, joint_states.position)) - set_values = list(position_dict.values())[:-2] - set_values.append(list(position_dict.values())[-1] * 2) - config.update( - **dict( - zip([key for key in config.keys() if key != "groups"], set_values) - ) - ) - - # Write joint positions to controller topics. - if level > -1 and level < 7: - self.arm_pubs[level].publish(list(config.values())[level]) - elif level in [7, 8] and self.gripper_connected: - move_goal = MoveGoal(width=config["width"], speed=config["speed"]) - self.gripper_move_client.send_goal(move_goal) - result = self.gripper_move_client.wait_for_result() - if not result: - rospy.logerr("Something went wrong while setting the gripper commands.") - elif level in [7, 8] and not self.gripper_connected: - rospy.logwarn_once( - "Gripper commands not applied since the gripper command action was not " - "found." - ) - return config - - -if __name__ == "__main__": - rospy.init_node("joint_position_reconfig_server", anonymous=False) - - position_dyn_server = JointPositionDynamicReconfigureServer() - - rospy.spin() diff --git a/panda_gazebo/tests/manual/moveit_commander_test.py b/panda_gazebo/tests/manual/moveit_commander_test.py index ab0db0e3..1991b0b1 100644 --- a/panda_gazebo/tests/manual/moveit_commander_test.py +++ b/panda_gazebo/tests/manual/moveit_commander_test.py @@ -1,23 +1,13 @@ #!/usr/bin/env python3 -"""Small script for testing capabilities of the Moveit commander. -""" +"""Small script for testing capabilities of the MoveIt commander.""" import sys +import math +from geometry_msgs.msg import Pose import moveit_commander import moveit_msgs.msg import rospy -try: - from math import pi, tau -except ImportError: # For Python 2 compatibility - from math import pi, sqrt - - tau = 2.0 * pi - - def dist(p, q): - return sqrt(sum((p_i - q_i) ** 2.0 for p_i, q_i in zip(p, q))) - - if __name__ == "__main__": # Initialise MoveIt commander and node. moveit_commander.roscpp_initialize(sys.argv) @@ -55,55 +45,55 @@ def dist(p, q): 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] = -tau / 8 - # joint_goal[2] = 0 - # joint_goal[3] = -tau / 4 - # joint_goal[4] = 0 - # joint_goal[5] = tau / 6 # 1/6 of a turn - # joint_goal[6] = 0 - - # # METHOD 1 - # # -- UNCOMMENT BELOW TO SEE THE SACLING 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 SACLING 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 = geometry_msgs.msg.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 = 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 hand goal -- hand_move_group.set_joint_value_target([0.04, 0.04]) diff --git a/panda_gazebo/tests/manual/moveit_server_dynamic_reconfigure_test.py b/panda_gazebo/tests/manual/moveit_server_dynamic_reconfigure_test.py index daf742eb..3dd2c903 100755 --- a/panda_gazebo/tests/manual/moveit_server_dynamic_reconfigure_test.py +++ b/panda_gazebo/tests/manual/moveit_server_dynamic_reconfigure_test.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 -"""Small node that spins up a dynamic reconfigure server that can be used to change the -parameters of the panda_moveit_planner_server. -""" +"""Node to dynamically reconfigure 'panda_moveit_planner_server' parameters.""" + import rospy from dynamic_reconfigure.server import Server diff --git a/panda_gazebo/tests/manual/panda_control_server_test.py b/panda_gazebo/tests/manual/panda_control_server_test.py index 1fd6ab71..efd931a2 100644 --- a/panda_gazebo/tests/manual/panda_control_server_test.py +++ b/panda_gazebo/tests/manual/panda_control_server_test.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 -"""Script used to manually test the '/panda_control_server' control services""" -import sys +"""Script used to manually test the '/panda_control_server' control services.""" import actionlib import rospy diff --git a/setup.cfg b/setup.cfg index 3c6e9b68..04273589 100644 --- a/setup.cfg +++ b/setup.cfg @@ -3,6 +3,7 @@ max-line-length = 89 exclude = .git, __pycache__, + build, sandbox, docs, tests, @@ -10,6 +11,8 @@ exclude = node_modules, franka_ros, panda_moveit_config + panda_gazebo/setup.py per-file-ignores = __init__.py: F401, E501 max-complexity = 10 +extend-ignore = E203, W503, D400, D401, D205