diff --git a/franka_gazebo/CMakeLists.txt b/franka_gazebo/CMakeLists.txt index d3aa771a1..09c858027 100644 --- a/franka_gazebo/CMakeLists.txt +++ b/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) diff --git a/franka_gazebo/config/EffortControlTestDynReconf.cfg b/franka_gazebo/config/EffortControlTestDynReconf.cfg new file mode 100644 index 000000000..5ff8c7256 --- /dev/null +++ b/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")) diff --git a/franka_gazebo/config/joint_effort_controllers.yaml b/franka_gazebo/config/joint_effort_controllers.yaml new file mode 100644 index 000000000..54b058214 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/franka_gazebo/launch/load_joint_effort_controllers.launch b/franka_gazebo/launch/load_joint_effort_controllers.launch new file mode 100644 index 000000000..3ded0eab3 --- /dev/null +++ b/franka_gazebo/launch/load_joint_effort_controllers.launch @@ -0,0 +1,9 @@ + + + + + + + + \ No newline at end of file diff --git a/franka_gazebo/launch/panda.launch b/franka_gazebo/launch/panda.launch index 92b1a7d35..2a97255fe 100644 --- a/franka_gazebo/launch/panda.launch +++ b/franka_gazebo/launch/panda.launch @@ -7,6 +7,8 @@ + + @@ -37,6 +39,7 @@ + catkin gazebo_dev + dynamic_reconfigure gazebo_ros gazebo_ros_control roscpp diff --git a/franka_gazebo/scripts/joint_effort_dynamic_reconfigure_server.py b/franka_gazebo/scripts/joint_effort_dynamic_reconfigure_server.py new file mode 100755 index 000000000..be5e15386 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/franka_gazebo/scripts/print_collision_info.py b/franka_gazebo/scripts/print_collision_info.py new file mode 100644 index 000000000..a6f8a3367 --- /dev/null +++ b/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) diff --git a/src.code-workspace b/src.code-workspace new file mode 100644 index 000000000..b3d4f60ce --- /dev/null +++ b/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" + } +} \ No newline at end of file