Skip to content
Snippets Groups Projects
Commit c5152e43 authored by Florian Walch's avatar Florian Walch
Browse files

Update example controllers

parent 1cbfbbcc
No related branches found
No related tags found
No related merge requests found
Showing
with 70 additions and 84 deletions
joint_velocity_example_controller: joint_velocity_example_controller:
type: franka_example_controllers/JointVelocityExampleController 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: joint_position_example_controller:
type: franka_example_controllers/JointPositionExampleController 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: cartesian_pose_example_controller:
type: franka_example_controllers/CartesianPoseExampleController type: franka_example_controllers/CartesianPoseExampleController
arm_id: panda
cartesian_velocity_example_controller: cartesian_velocity_example_controller:
type: franka_example_controllers/CartesianVelocityExampleController type: franka_example_controllers/CartesianVelocityExampleController
arm_id: panda
model_example_controller: model_example_controller:
type: franka_example_controllers/ModelExampleController type: franka_example_controllers/ModelExampleController
......
...@@ -18,8 +18,7 @@ class JointPositionExampleController : public controller_interface::MultiInterfa ...@@ -18,8 +18,7 @@ class JointPositionExampleController : public controller_interface::MultiInterfa
hardware_interface::PositionJointInterface> { hardware_interface::PositionJointInterface> {
public: public:
bool init(hardware_interface::RobotHW* robot_hardware, bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle, ros::NodeHandle& node_handle) override;
ros::NodeHandle& /* controller_node_handle */) override;
void starting(const ros::Time&) override; void starting(const ros::Time&) override;
void update(const ros::Time&, const ros::Duration& period) override; void update(const ros::Time&, const ros::Duration& period) override;
......
...@@ -19,8 +19,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa ...@@ -19,8 +19,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa
franka_hw::FrankaStateInterface> { franka_hw::FrankaStateInterface> {
public: public:
bool init(hardware_interface::RobotHW* robot_hardware, bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle, ros::NodeHandle& node_handle) override;
ros::NodeHandle& /* controller_node_handle */) override;
void update(const ros::Time&, const ros::Duration& period) override; void update(const ros::Time&, const ros::Duration& period) override;
void starting(const ros::Time&) override; void starting(const ros::Time&) override;
void stopping(const ros::Time&) override; void stopping(const ros::Time&) override;
......
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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 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="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" /> <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
</group>
</launch> </launch>
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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"/> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
</group>
</launch> </launch>
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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"/> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
</group>
</launch> </launch>
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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 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" /> <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
</group>
</launch> </launch>
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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"/> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
</group>
</launch> </launch>
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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"/> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
</group>
</launch> </launch>
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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"/> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
</group>
</launch> </launch>
<?xml version="1.0" ?> <?xml version="1.0" ?>
<launch> <launch>
<arg name="robot_ip" default="robot.franka.de" /> <arg name="robot_ip" default="robot.franka.de" />
<arg name="arm_id" value="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<include file="$(find franka_control)/launch/franka_control.launch" > <include file="$(find franka_control)/launch/franka_control.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" /> <arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="arm_id" value="$(arg arm_id)" />
<arg name="load_gripper" value="$(arg load_gripper)" /> <arg name="load_gripper" value="$(arg load_gripper)" />
</include> </include>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" /> <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 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"/> <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
</group>
</launch> </launch>
...@@ -115,7 +115,7 @@ Visualization Manager: ...@@ -115,7 +115,7 @@ Visualization Manager:
Show Axes: false Show Axes: false
Show Descriptions: true Show Descriptions: true
Show Visual Aids: false Show Visual Aids: false
Update Topic: /panda/equilibrium_pose_marker/update Update Topic: /equilibrium_pose_marker/update
Value: true Value: true
Enabled: true Enabled: true
Global Options: Global Options:
......
...@@ -18,8 +18,8 @@ pose_pub = None ...@@ -18,8 +18,8 @@ pose_pub = None
position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]] position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]]
def publisherCallback(msg, arm_id): def publisherCallback(msg, link_name):
marker_pose.header.frame_id = arm_id + "_link0" marker_pose.header.frame_id = link_name
marker_pose.header.stamp = rospy.Time(0) marker_pose.header.stamp = rospy.Time(0)
pose_pub.publish(marker_pose) pose_pub.publish(marker_pose)
...@@ -61,7 +61,7 @@ if __name__ == "__main__": ...@@ -61,7 +61,7 @@ if __name__ == "__main__":
state_sub = rospy.Subscriber("franka_state_controller/franka_states", state_sub = rospy.Subscriber("franka_state_controller/franka_states",
FrankaState, franka_state_callback) FrankaState, franka_state_callback)
listener = tf.TransformListener() listener = tf.TransformListener()
arm_id = rospy.get_param("arm_id") link_name = rospy.get_param("~link_name")
# Get initial pose for the interactive marker # Get initial pose for the interactive marker
while not initial_pose_found: while not initial_pose_found:
...@@ -69,10 +69,10 @@ if __name__ == "__main__": ...@@ -69,10 +69,10 @@ if __name__ == "__main__":
state_sub.unregister() state_sub.unregister()
pose_pub = rospy.Publisher( pose_pub = rospy.Publisher(
"/equilibrium_pose", PoseStamped, queue_size=10) "equilibrium_pose", PoseStamped, queue_size=10)
server = InteractiveMarkerServer("equilibrium_pose_marker") server = InteractiveMarkerServer("equilibrium_pose_marker")
int_marker = InteractiveMarker() 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.scale = 0.3
int_marker.name = "equilibrium_pose" int_marker.name = "equilibrium_pose"
int_marker.description = ("Equilibrium Pose\nBE CAREFUL! " int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
...@@ -82,7 +82,7 @@ if __name__ == "__main__": ...@@ -82,7 +82,7 @@ if __name__ == "__main__":
int_marker.pose = marker_pose.pose int_marker.pose = marker_pose.pose
# run pose publisher # run pose publisher
rospy.Timer(rospy.Duration(0.005), rospy.Timer(rospy.Duration(0.005),
lambda msg: publisherCallback(msg, arm_id)) lambda msg: publisherCallback(msg, link_name))
# insert a box # insert a box
control = InteractiveMarkerControl() control = InteractiveMarkerControl()
......
...@@ -13,8 +13,7 @@ ...@@ -13,8 +13,7 @@
namespace franka_example_controllers { namespace franka_example_controllers {
bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware, bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle, ros::NodeHandle& node_handle) {
ros::NodeHandle& /* controller_node_handle */) {
position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>(); position_joint_interface_ = robot_hardware->get<hardware_interface::PositionJointInterface>();
if (position_joint_interface_ == nullptr) { if (position_joint_interface_ == nullptr) {
ROS_ERROR( ROS_ERROR(
...@@ -22,7 +21,7 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har ...@@ -22,7 +21,7 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har
return false; return false;
} }
std::vector<std::string> joint_names; 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"); ROS_ERROR("JointPositionExampleController: Could not parse joint names");
} }
if (joint_names.size() != 7) { if (joint_names.size() != 7) {
......
...@@ -13,8 +13,7 @@ ...@@ -13,8 +13,7 @@
namespace franka_example_controllers { namespace franka_example_controllers {
bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware, bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle, ros::NodeHandle& node_handle) {
ros::NodeHandle& /* controller_node_handle */) {
velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>(); velocity_joint_interface_ = robot_hardware->get<hardware_interface::VelocityJointInterface>();
if (velocity_joint_interface_ == nullptr) { if (velocity_joint_interface_ == nullptr) {
ROS_ERROR( ROS_ERROR(
...@@ -22,7 +21,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har ...@@ -22,7 +21,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
return false; return false;
} }
std::vector<std::string> joint_names; 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"); ROS_ERROR("JointVelocityExampleController: Could not parse joint names");
} }
if (joint_names.size() != 7) { if (joint_names.size() != 7) {
...@@ -41,12 +40,6 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har ...@@ -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>(); auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
if (state_interface == nullptr) { if (state_interface == nullptr) {
ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware"); ROS_ERROR("JointVelocityExampleController: Could not get state interface from hardware");
...@@ -54,7 +47,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har ...@@ -54,7 +47,7 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
} }
try { 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}}; 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++) { for (size_t i = 0; i < q_start.size(); i++) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment