From ade25510caa990f00c82f4b7e7067d5e04e403bd Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Mon, 20 Nov 2023 15:02:08 +0100
Subject: [PATCH] feat: add #226 test script

---
 .../scripts/set_random_joint_positions_226.py | 94 +++++++++++++++++++
 1 file changed, 94 insertions(+)
 create mode 100755 franka_gazebo/scripts/set_random_joint_positions_226.py

diff --git a/franka_gazebo/scripts/set_random_joint_positions_226.py b/franka_gazebo/scripts/set_random_joint_positions_226.py
new file mode 100755
index 000000000..1d978041d
--- /dev/null
+++ b/franka_gazebo/scripts/set_random_joint_positions_226.py
@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+"""Small example script to show that the joint position are not pushed ouside the joint
+limits when the `set_franka_model_configuration` service that was implemented in
+https://github.com/frankaemika/franka_ros/pull/226 is used.
+"""  # noqa: E501
+
+import numpy as np
+import rospy
+from franka_msgs.srv import SetJointConfiguration, SetJointConfigurationRequest
+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, -2.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_franka_model_configuration'
+    service.
+
+    Args:
+        joint_positions (list): A list of joint positions.
+    """
+    try:
+        set_model_configuration = rospy.ServiceProxy(
+            "/set_franka_model_configuration", SetJointConfiguration
+        )
+        request = SetJointConfigurationRequest()
+        request.configuration.name = JOINT_NAMES
+        request.configuration.position = 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_franka_model_configuration service: %s", e)
+
+
+if __name__ == "__main__":
+    rospy.init_node("set_random_joint_positions")
+
+    # x = True
+    # while x:
+    #     pass
+
+    # 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)