From c5152e43e4f0da364fa9752b58b36c623b5d660c Mon Sep 17 00:00:00 2001 From: Florian Walch <florian.walch@franka.de> Date: Thu, 15 Mar 2018 14:43:01 +0100 Subject: [PATCH] Update example controllers --- .../config/franka_example_controllers.yaml | 20 ++++++++++++++++++- .../joint_position_example_controller.h | 3 +-- .../joint_velocity_example_controller.h | 3 +-- ...tesian_impedance_example_controller.launch | 17 ++++++++-------- .../cartesian_pose_example_controller.launch | 11 ++++------ ...rtesian_velocity_example_controller.launch | 11 ++++------ .../launch/force_example_controller.launch | 13 +++++------- .../joint_impedance_example_controller.launch | 11 ++++------ .../joint_position_example_controller.launch | 11 ++++------ .../joint_velocity_example_controller.launch | 11 ++++------ .../launch/model_example_controller.launch | 11 ++++------ .../rviz/franka_description_with_marker.rviz | 2 +- .../scripts/interactive_marker.py | 12 +++++------ .../src/joint_position_example_controller.cpp | 5 ++--- .../src/joint_velocity_example_controller.cpp | 13 +++--------- 15 files changed, 70 insertions(+), 84 deletions(-) diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml index b6ffea5..47fbc9d 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 daf4c4d..b4e252b 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 1c36f4a..a09580a 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 a1417a0..871c1b3 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 ddc2184..4f2973e 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 e1df8cd..d4a6579 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 0187bbd..37525c7 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 3107896..a9ce416 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 f68b812..96193d7 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 ed8925f..9353eff 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 e7475bd..9b35470 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 d48a6db..eacde25 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 379a7ca..376ac18 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 b630014..3ba12b8 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 dbe4c23..ad01ad6 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++) { -- GitLab