From c09abfad7dd00ed9d1024f1ca0528eff94411f1f Mon Sep 17 00:00:00 2001 From: rickstaa Date: Mon, 4 Sep 2023 16:59:38 +0200 Subject: [PATCH] feat: add gazebo 'set_model_config' problem example scripts --- franka_gazebo/scripts/log_joint_violations.py | 77 +++++++++++++++ .../scripts/set_random_joint_positions.py | 93 +++++++++++++++++++ 2 files changed, 170 insertions(+) create mode 100755 franka_gazebo/scripts/log_joint_violations.py create mode 100755 franka_gazebo/scripts/set_random_joint_positions.py diff --git a/franka_gazebo/scripts/log_joint_violations.py b/franka_gazebo/scripts/log_joint_violations.py new file mode 100755 index 000000000..a5d2b19e4 --- /dev/null +++ b/franka_gazebo/scripts/log_joint_violations.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python +"""Small example script to show that the joint position is sometimes pushed outside of +the joint limit when the Gazebo 'set_model_configuration' service is used. This script +periodically checks whether the joint positions are within the joint limits and logs a +warning if not. This script is meant to be run together with the +`set_random_joint_positions.py`` script. +""" # noqa: E501 + +import numpy as np +import rospy +from sensor_msgs.msg import JointState + +# Robot info. +JOINT_NAMES = [ + "panda_joint1", + "panda_joint2", + "panda_joint3", + "panda_joint4", + "panda_joint5", + "panda_joint6", + "panda_joint7", + "panda_finger_joint1", + "panda_finger_joint2", +] +JOINT_LIMITS = { + "min": [-2.8973, -1.7628, -2.8973, -3.071, -2.8973, -0.0175, -2.8973, 0.0, 0.0], + "max": [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973, 0.04, 0.04], +} + + +def check_joint_violations(): + """Check if the joint positions are within the joint limits and log a warning if + not. + """ + try: + joint_state = rospy.wait_for_message("/joint_states", JointState) + except rospy.exceptions.ROSException as e: + rospy.logerr("Failed to receive joint state message: %s", e) + return + + violated_joints = { + JOINT_NAMES[i]: joint_position + for i, joint_position in enumerate(joint_state.position) + if ( + joint_position < JOINT_LIMITS["min"][i] + or joint_position > JOINT_LIMITS["max"][i] + ) + } + if violated_joints: + rospy.logwarn( + "The following joints are outside of the joint limits: %s - %s", + list(violated_joints.keys()), + list(violated_joints.values()), + ) + else: + rospy.loginfo("No joints are outside of the joint limits.") + + +if __name__ == "__main__": + rospy.init_node("show_gazebo_set_model_configuration_problem") + + # Checks whether the joint positions are within the joint limits every seconds. + rate = rospy.Rate(1) + while not rospy.is_shutdown(): + joint_positions = [ + np.random.uniform(low, high) + for low, high in zip(JOINT_LIMITS["min"], JOINT_LIMITS["max"]) + ] + try: + check_joint_violations() + except rospy.exceptions.ROSException as e: + rospy.logerr("Failed to check joint voilations: %s", e) + rate.sleep() + + # Gracefully shut down the node. + rospy.loginfo("Shutting down...") + rospy.signal_shutdown("Finished") diff --git a/franka_gazebo/scripts/set_random_joint_positions.py b/franka_gazebo/scripts/set_random_joint_positions.py new file mode 100755 index 000000000..f6da312ba --- /dev/null +++ b/franka_gazebo/scripts/set_random_joint_positions.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python + +"""Small example script to show that the joint position is sometimes pushed outside of +the joint limit when the Gazebo 'set_model_configuration' service is used. This script +periodically sets random joint positions within the joint limits. This script is meant +to be run together with the `check_joint_violations.py`` script. +""" # noqa: E501 + +import numpy as np +import rospy +from gazebo_msgs.srv import SetModelConfiguration, SetModelConfigurationRequest +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +# Robot info. +JOINT_NAMES = [ + "panda_joint1", + "panda_joint2", + "panda_joint3", + "panda_joint4", + "panda_joint5", + "panda_joint6", + "panda_joint7", + "panda_finger_joint1", + "panda_finger_joint2", +] +JOINT_LIMITS = { + "min": [-2.8973, -1.7628, -2.8973, -3.071, -2.8973, -0.0175, -2.8973, 0.0, 0.0], + "max": [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973, 0.04, 0.04], +} +JOINT_INITIAL_POSITIONS = [0.0] * len(JOINT_NAMES) + + +def set_random_joint_positions(joint_positions): + """Set the joint positions of the robot using the Gazebo 'set_model_configuration' + service. + + Args: + joint_positions (list): A list of joint positions. + """ + try: + set_model_configuration = rospy.ServiceProxy( + "/gazebo/set_model_configuration", SetModelConfiguration + ) + request = SetModelConfigurationRequest() + request.model_name = "panda" + request.urdf_param_name = "robot_description" + request.joint_names = JOINT_NAMES + request.joint_positions = joint_positions + set_model_configuration(request) + rospy.loginfo("Set joint positions to: %s", joint_positions) + except rospy.ServiceException as e: + rospy.logerr("Failed to call set_model_configuration service: %s", e) + + +if __name__ == "__main__": + rospy.init_node("set_random_joint_positions") + + # Set random joint positions within the joint limits. + try: + joint_positions = [ + np.random.uniform(low, high) + for low, high in zip(JOINT_LIMITS["min"], JOINT_LIMITS["max"]) + ] + set_random_joint_positions(joint_positions) + except rospy.exceptions.ROSException as e: + rospy.logerr("Failed to set random joint positions: %s", e) + + # Repeat the above every 5 seconds. + rate = rospy.Rate(0.2) + while not rospy.is_shutdown(): + rate.sleep() + try: + joint_positions = [ + np.random.uniform(low, high) + for low, high in zip(JOINT_LIMITS["min"], JOINT_LIMITS["max"]) + ] + set_random_joint_positions(joint_positions) + except rospy.exceptions.ROSException as e: + rospy.logerr("Failed to set random joint positions: %s", e) + + # Reset the joint positions to their initial values when the node is shut down. + joint_trajectory = JointTrajectory() + joint_trajectory.joint_names = JOINT_NAMES + joint_trajectory.points.append( + JointTrajectoryPoint(positions=JOINT_INITIAL_POSITIONS) + ) + try: + pub = rospy.Publisher( + "/panda_arm_controller/command", JointTrajectory, queue_size=10 + ) + pub.publish(joint_trajectory) + except rospy.exceptions.ROSException as e: + rospy.logerr("Failed to reset joint positions: %s", e)