feat: add gazebo collision log script

franka_gazebo/CMakeLists.txt

@@ -35,6 +35,11 @@ find_package(catkin REQUIRED COMPONENTS
   franka_msgs
   franka_gripper
   franka_example_controllers
+  dynamic_reconfigure
+)
+
+generate_dynamic_reconfigure_options(
+  config/EffortControlTestDynReconf.cfg
 )
 
 find_package(Franka 0.7.0 REQUIRED)

franka_gazebo/config/EffortControlTestDynReconf.cfg

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+# NOTE: Config for changing the joint efforts.
+
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t
+
+PACKAGE = "franka_gazebo"
+
+gen = ParameterGenerator()
+
+# Generate arm effort control parameters
+arm = gen.add_group("arm")
+arm.add("joint1_effort", double_t, 0, "Joint 1 effort control command", 0, -87.0, 87.0)
+arm.add("joint2_effort", double_t, 1, "Joint 2 effort control command", 0, -87.0, 87.0)
+arm.add("joint3_effort", double_t, 2, "Joint 3 effort control command", 0, -87.0, 87.0)
+arm.add("joint4_effort", double_t, 3, "Joint 4 effort control command", 0, -87.0, 87.0)
+arm.add("joint5_effort", double_t, 4, "Joint 5 effort control command", 0, -12.0, 12.0)
+arm.add("joint6_effort", double_t, 5, "Joint 6 effort control command", 0, -12.0, 12.0)
+arm.add("joint7_effort", double_t, 6, "Joint 7 effort control command", 0, -12.0, 12.0)
+
+# Generate hand effort control parameters
+hand = gen.add_group("hand")
+hand.add(
+    "width",
+    double_t,
+    7,
+    "Gripper width",
+    0,
+    0.0,
+    0.08,
+)
+hand.add(
+    "speed",
+    double_t,
+    8,
+    "Gripper speed",
+    0.2,
+    0.0,
+    0.2,
+)
+
+# Generate the necessary files and exit the program
+exit(gen.generate(PACKAGE, "panda_test", "JointEffort"))

franka_gazebo/config/joint_effort_controllers.yaml

@@ -0,0 +1,23 @@
+# 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

franka_gazebo/launch/load_joint_effort_controllers.launch

@@ -0,0 +1,9 @@
+<!-- Launch file for launching the robot controllers -->
+<launch>
+  <!--Load controller parameters-->
+  <rosparam file="$(find franka_gazebo)/config/joint_effort_controllers.yaml" command="load"/>
+
+  <!--Load the controllers-->
+  <node name="joint_effort_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="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"/>
+</launch>

franka_gazebo/launch/panda.launch

@@ -7,6 +7,8 @@
   <arg name="paused"      default="false" doc="Should the simulation directly be stopped at 0s?" />
   <arg name="world"       default="worlds/empty.world" doc="Filename to a SDF World for gazebo to use" />
   <arg name="rviz"        default="false" doc="Should RVIz be launched?" />
+  <!--Phyics engines: dart|ode-->
+  <arg name="physics" default="dart" doc="The physics engine used by gazebo"/>
 
   <!-- Robot Customization -->
   <arg name="arm_id"      default="panda" doc="Name of the panda robot to spawn" />
@@ -37,6 +39,7 @@
     <arg name="paused" value="true"/>
     <arg name="gui" value="$(eval not arg('headless'))"/>
     <arg name="use_sim_time" value="true"/>
+    <arg name="physics" value="$(arg physics)"/>
   </include>
 
   <param name="robot_description"

franka_gazebo/package.xml

@@ -16,6 +16,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>gazebo_dev</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
   <exec_depend>gazebo_ros</exec_depend>
   <depend>gazebo_ros_control</depend>
   <depend>roscpp</depend>

franka_gazebo/scripts/joint_effort_dynamic_reconfigure_server.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python3
+"""Small node that spins up a dynamic reconfigure server that can be used to change the
+joint efforts.
+"""
+import rospy
+from dynamic_reconfigure.server import Server
+from franka_gazebo.cfg import JointEffortConfig
+from std_msgs.msg import Float64
+import actionlib
+from franka_gripper.msg import MoveAction, MoveGoal
+from sensor_msgs.msg import JointState
+
+
+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()

franka_gazebo/scripts/print_collision_info.py

@@ -0,0 +1,118 @@
+"""Simple script that prints some information about the collision information that is
+published on the 'franka_states' topic.
+"""
+
+import rospy
+from franka_msgs.msg import FrankaState
+import time
+from sensor_msgs.msg import JointState
+
+SLEEP_TIME = 1
+CARTESIAN_COORDINATES = ["x", "y", "z", "rx", "ry", "rz", "rw"]
+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],
+}
+
+color2num = dict(
+    gray=30,
+    red=31,
+    green=32,
+    yellow=33,
+    blue=34,
+    magenta=35,
+    cyan=36,
+    white=37,
+    crimson=38,
+)
+
+
+def colorize(string, color, bold=False, highlight=False):
+    """Return string surrounded by appropriate terminal color codes to
+    print colorized text.  Valid colors: gray, red, green, yellow,
+    blue, magenta, cyan, white, crimson
+    """
+
+    attr = []
+    num = color2num[color]
+    if highlight:
+        num += 10
+    attr.append(str(num))
+    if bold:
+        attr.append("1")
+    attrs = ";".join(attr)
+    return "\x1b[%sm%s\x1b[0m" % (attrs, string)
+
+
+if __name__ == "__main__":
+    rospy.init_node(
+        "test_gazebo_collision_behaviour", anonymous=True, log_level=rospy.INFO
+    )
+    print("")
+    print(">>>> Starting joint collision information logger <<<<")
+    print(f"Joint min limits: {JOINT_LIMITS['min']}")
+    print(f"Joint max limits: {JOINT_LIMITS['max']}")
+
+    while True:
+        # Retrieve franka_states and joint_states
+        franka_states = rospy.wait_for_message(
+            "/franka_state_controller/franka_states", FrankaState, timeout=10
+        )
+        joint_states = rospy.wait_for_message("/joint_states", JointState, timeout=10)
+        cartesian_collision = franka_states.cartesian_collision
+        joint_collision = franka_states.joint_collision
+
+        # Print collision information
+        limits_respected = [
+            position >= JOINT_LIMITS["min"][idx]
+            and position <= JOINT_LIMITS["max"][idx]
+            for idx, position in enumerate(joint_states.position)
+        ]
+        print("")
+        print(
+            "Joint positions:\n{}".format(
+                "\n".join(
+                    [
+                        colorize(f"    {item}", color="white", bold=True)
+                        if bool
+                        else colorize(f"    {item}", color="yellow", bold=True)
+                        for item, bool in zip(
+                            list(joint_states.position), limits_respected
+                        )
+                    ]
+                )
+            )
+        )
+        if any(cartesian_collision):
+            collision_directions = [
+                coordinate
+                for coordinate, bool in zip(CARTESIAN_COORDINATES, cartesian_collision)
+                if bool
+            ]
+            rospy.logerr(
+                f"Cartesian collision detected in direction '{collision_directions}'."
+            )
+        else:
+            rospy.loginfo("No cartesian collision detected.")
+        if any(joint_collision):
+            collision_joints = [
+                coordinate
+                for coordinate, bool in zip(JOINT_NAMES, joint_collision)
+                if bool
+            ]
+            rospy.logerr(f"Joint collision detected in joints '{collision_joints}'.")
+        else:
+            rospy.loginfo("No joint collision detected.")
+
+        time.sleep(SLEEP_TIME)

src.code-workspace

@@ -0,0 +1,20 @@
+{
+  "settings": {
+    "workbench.colorCustomizations": {
+      "activityBar.activeBackground": "#52db44",
+      "activityBar.activeBorder": "#5f6be0",
+      "activityBar.background": "#52db44",
+      "activityBar.foreground": "#15202b",
+      "activityBar.inactiveForeground": "#15202b99",
+      "activityBarBadge.background": "#5f6be0",
+      "activityBarBadge.foreground": "#e7e7e7",
+      "sash.hoverBorder": "#52db44",
+      "tab.activeBorder": "#52db44",
+      "titleBar.activeBackground": "#35c626",
+      "titleBar.activeForeground": "#15202b",
+      "titleBar.inactiveBackground": "#35c62699",
+      "titleBar.inactiveForeground": "#15202b99"
+    },
+    "peacock.color": "#35c626"
+  }
+}