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