diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml index b6ffea56a858873c31f77a9faf76ccef9595b5f0..47fbc9d762a478b9c37b81bd0fdcb2566e05753a 100644 --- a/franka_example_controllers/config/franka_example_controllers.yaml +++ b/franka_example_controllers/config/franka_example_controllers.yaml @@ -1,14 +1,32 @@ joint_velocity_example_controller: type: franka_example_controllers/JointVelocityExampleController + joint_names: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 joint_position_example_controller: type: franka_example_controllers/JointPositionExampleController + joint_names: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 cartesian_pose_example_controller: type: franka_example_controllers/CartesianPoseExampleController + arm_id: panda cartesian_velocity_example_controller: type: franka_example_controllers/CartesianVelocityExampleController + arm_id: panda model_example_controller: type: franka_example_controllers/ModelExampleController @@ -56,7 +74,7 @@ joint_impedance_example_controller: radius: 0.1 acceleration_time: 2.0 vel_max: 0.15 - publish_rate: 10.0 + publish_rate: 10.0 coriolis_factor: 1.0 cartesian_impedance_example_controller: diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h index daf4c4d81cb7d66ee7f4412d4202e4839452fa9f..b4e252b88d16505bb008731e4f9749b111c802f0 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h @@ -18,8 +18,7 @@ class JointPositionExampleController : public controller_interface::MultiInterfa hardware_interface::PositionJointInterface> { public: bool init(hardware_interface::RobotHW* robot_hardware, - ros::NodeHandle& root_node_handle, - ros::NodeHandle& /* controller_node_handle */) override; + ros::NodeHandle& node_handle) override; void starting(const ros::Time&) override; void update(const ros::Time&, const ros::Duration& period) override; diff --git a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h index 1c36f4a971c7a92f58bd184e7a26bc0ad3a34a73..a09580a4944710c92e2eaf8bf7635c69d519d156 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h @@ -19,8 +19,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa franka_hw::FrankaStateInterface> { public: bool init(hardware_interface::RobotHW* robot_hardware, - ros::NodeHandle& root_node_handle, - ros::NodeHandle& /* controller_node_handle */) override; + ros::NodeHandle& node_handle) override; void update(const ros::Time&, const ros::Duration& period) override; void starting(const ros::Time&) override; void stopping(const ros::Time&) override; diff --git a/franka_example_controllers/launch/cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/cartesian_impedance_example_controller.launch index a1417a072444683c39a080d544f7d480a0d6c196..871c1b3fa8b2f2f635046133b2a63537be5948a3 100644 --- a/franka_example_controllers/launch/cartesian_impedance_example_controller.launch +++ b/franka_example_controllers/launch/cartesian_impedance_example_controller.launch @@ -1,18 +1,17 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_impedance_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/> - <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen" /> - <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" /> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_impedance_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/> + <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen"> + <param name="link_name" value="panda_link0" /> + </node> + <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" /> </launch> diff --git a/franka_example_controllers/launch/cartesian_pose_example_controller.launch b/franka_example_controllers/launch/cartesian_pose_example_controller.launch index ddc2184e303c2e78c77d9427664fd77471a96c11..4f2973e81d1db4ee1293a1291e1c34b45aaa21be 100644 --- a/franka_example_controllers/launch/cartesian_pose_example_controller.launch +++ b/franka_example_controllers/launch/cartesian_pose_example_controller.launch @@ -1,16 +1,13 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_pose_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_pose_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> </launch> diff --git a/franka_example_controllers/launch/cartesian_velocity_example_controller.launch b/franka_example_controllers/launch/cartesian_velocity_example_controller.launch index e1df8cda955ea91ac9039aaeaa03be94ba3b5662..d4a657973311d6ccb4dafdb6fabc0ab58026f810 100644 --- a/franka_example_controllers/launch/cartesian_velocity_example_controller.launch +++ b/franka_example_controllers/launch/cartesian_velocity_example_controller.launch @@ -1,16 +1,13 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_velocity_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_velocity_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> </launch> diff --git a/franka_example_controllers/launch/force_example_controller.launch b/franka_example_controllers/launch/force_example_controller.launch index 0187bbda9c8e5f501cab2865a6a4f69a9e73e652..37525c725e95d56f38619cc6905ecfa7294385b6 100644 --- a/franka_example_controllers/launch/force_example_controller.launch +++ b/franka_example_controllers/launch/force_example_controller.launch @@ -1,17 +1,14 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="force_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> - <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" /> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="force_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> + <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" /> </launch> diff --git a/franka_example_controllers/launch/joint_impedance_example_controller.launch b/franka_example_controllers/launch/joint_impedance_example_controller.launch index 310789644061dd75835a0bb3083f557d6c590d5e..a9ce4169854978ecc21e89b05e82dbb52734bf97 100644 --- a/franka_example_controllers/launch/joint_impedance_example_controller.launch +++ b/franka_example_controllers/launch/joint_impedance_example_controller.launch @@ -1,16 +1,13 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_impedance_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_impedance_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> </launch> diff --git a/franka_example_controllers/launch/joint_position_example_controller.launch b/franka_example_controllers/launch/joint_position_example_controller.launch index f68b812fd0f15632c725d6966f7eed466f96bc42..96193d79d4a719616016ee157f2447c58986fb12 100644 --- a/franka_example_controllers/launch/joint_position_example_controller.launch +++ b/franka_example_controllers/launch/joint_position_example_controller.launch @@ -1,16 +1,13 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_position_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_position_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> </launch> diff --git a/franka_example_controllers/launch/joint_velocity_example_controller.launch b/franka_example_controllers/launch/joint_velocity_example_controller.launch index ed8925f19d5cd321226ece8a08261642912aeaed..9353effdf9bb2f6334262d21acfe680a89e2b37c 100644 --- a/franka_example_controllers/launch/joint_velocity_example_controller.launch +++ b/franka_example_controllers/launch/joint_velocity_example_controller.launch @@ -1,16 +1,13 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_velocity_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_velocity_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> </launch> diff --git a/franka_example_controllers/launch/model_example_controller.launch b/franka_example_controllers/launch/model_example_controller.launch index e7475bdc480adac14f423c07015af6eb268dc772..9b354709b3a50a7ac044a0216b971d4bf750837c 100644 --- a/franka_example_controllers/launch/model_example_controller.launch +++ b/franka_example_controllers/launch/model_example_controller.launch @@ -1,16 +1,13 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" default="robot.franka.de" /> - <arg name="arm_id" value="panda" /> <arg name="load_gripper" default="true" /> <include file="$(find franka_control)/launch/franka_control.launch" > <arg name="robot_ip" value="$(arg robot_ip)" /> - <arg name="arm_id" value="$(arg arm_id)" /> <arg name="load_gripper" value="$(arg load_gripper)" /> </include> - <group ns="$(arg arm_id)"> - <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="model_example_controller"/> - <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> - </group> + + <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="model_example_controller"/> + <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/> </launch> diff --git a/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz b/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz index d48a6db40c95fd1f65ee86e4d899bcb51bbe987c..eacde2590ab8a775a6658cbf0f6a4b96a105031f 100644 --- a/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz +++ b/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz @@ -115,7 +115,7 @@ Visualization Manager: Show Axes: false Show Descriptions: true Show Visual Aids: false - Update Topic: /panda/equilibrium_pose_marker/update + Update Topic: /equilibrium_pose_marker/update Value: true Enabled: true Global Options: diff --git a/franka_example_controllers/scripts/interactive_marker.py b/franka_example_controllers/scripts/interactive_marker.py index 379a7cadd5705b42cdd13f8c3485127292dfbc7b..376ac1803d1bf5655ff2db9b825baffecd1b4d81 100755 --- a/franka_example_controllers/scripts/interactive_marker.py +++ b/franka_example_controllers/scripts/interactive_marker.py @@ -18,8 +18,8 @@ pose_pub = None position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]] -def publisherCallback(msg, arm_id): - marker_pose.header.frame_id = arm_id + "_link0" +def publisherCallback(msg, link_name): + marker_pose.header.frame_id = link_name marker_pose.header.stamp = rospy.Time(0) pose_pub.publish(marker_pose) @@ -61,7 +61,7 @@ if __name__ == "__main__": state_sub = rospy.Subscriber("franka_state_controller/franka_states", FrankaState, franka_state_callback) listener = tf.TransformListener() - arm_id = rospy.get_param("arm_id") + link_name = rospy.get_param("~link_name") # Get initial pose for the interactive marker while not initial_pose_found: @@ -69,10 +69,10 @@ if __name__ == "__main__": state_sub.unregister() pose_pub = rospy.Publisher( - "/equilibrium_pose", PoseStamped, queue_size=10) + "equilibrium_pose", PoseStamped, queue_size=10) server = InteractiveMarkerServer("equilibrium_pose_marker") int_marker = InteractiveMarker() - int_marker.header.frame_id = arm_id + "_link0" + int_marker.header.frame_id = link_name int_marker.scale = 0.3 int_marker.name = "equilibrium_pose" int_marker.description = ("Equilibrium Pose\nBE CAREFUL! " @@ -82,7 +82,7 @@ if __name__ == "__main__": int_marker.pose = marker_pose.pose # run pose publisher rospy.Timer(rospy.Duration(0.005), - lambda msg: publisherCallback(msg, arm_id)) + lambda msg: publisherCallback(msg, link_name)) # insert a box control = InteractiveMarkerControl() diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp index b630014a099080be6107eb61861f1e18ca2cc9da..3ba12b8d80072bc1de24d83c334ed3d23115507f 100644 --- a/franka_example_controllers/src/joint_position_example_controller.cpp +++ b/franka_example_controllers/src/joint_position_example_controller.cpp @@ -13,8 +13,7 @@ namespace franka_example_controllers { bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware, - ros::NodeHandle& root_node_handle, - ros::NodeHandle& /* controller_node_handle */) { + ros::NodeHandle& node_handle) { position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>(); if (position_joint_interface_ == nullptr) { ROS_ERROR( @@ -22,7 +21,7 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har return false; } std::vector<std::string> joint_names; - if (!root_node_handle.getParam("joint_names", joint_names)) { + if (!node_handle.getParam("joint_names", joint_names)) { ROS_ERROR("JointPositionExampleController: Could not parse joint names"); } if (joint_names.size() != 7) { diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp index dbe4c235b2d75618b1c33f67fc7e30ca1d9b42f9..ad01ad6bd1a2d28a259e1fc24b73e60f159d053f 100644 --- a/franka_example_controllers/src/joint_velocity_example_controller.cpp +++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp @@ -13,8 +13,7 @@ namespace franka_example_controllers { bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware, - ros::NodeHandle& root_node_handle, - ros::NodeHandle& /* controller_node_handle */) { + ros::NodeHandle& node_handle) { velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>(); if (velocity_joint_interface_ == nullptr) { ROS_ERROR( @@ -22,7 +21,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har return false; } std::vector<std::string> joint_names; - if (!root_node_handle.getParam("joint_names", joint_names)) { + if (!node_handle.getParam("joint_names", joint_names)) { ROS_ERROR("JointVelocityExampleController: Could not parse joint names"); } if (joint_names.size() != 7) { @@ -41,12 +40,6 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har } } - std::string arm_id; - if (!root_node_handle.getParam("arm_id", arm_id)) { - ROS_ERROR("JointVelocityExampleController: Could not get parameter arm_id"); - return false; - } - auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>(); if (state_interface == nullptr) { ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware"); @@ -54,7 +47,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har } try { - auto state_handle = state_interface->getHandle(arm_id + "_robot"); + auto state_handle = state_interface->getHandle("panda_robot"); std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; for (size_t i = 0; i < q_start.size(); i++) {