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
Hide whitespace changes
Inline Side-by-side
Showing
with 70 additions and 84 deletions
franka_example_controllers/config/franka_example_controllers.yaml
+ 19
− 1
View file @ c5152e43
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:
......
franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
+ 1
− 2
View file @ c5152e43
......@@ -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;
......
franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h
+ 1
− 2
View file @ c5152e43
......@@ -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;
......
franka_example_controllers/launch/cartesian_impedance_example_controller.launch
+ 8
− 9
View file @ c5152e43
<?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>
franka_example_controllers/launch/cartesian_pose_example_controller.launch
+ 4
− 7
View file @ c5152e43
<?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>
franka_example_controllers/launch/cartesian_velocity_example_controller.launch
+ 4
− 7
View file @ c5152e43
<?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>
franka_example_controllers/launch/force_example_controller.launch
+ 5
− 8
View file @ c5152e43
<?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>
franka_example_controllers/launch/joint_impedance_example_controller.launch
+ 4
− 7
View file @ c5152e43
<?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>
franka_example_controllers/launch/joint_position_example_controller.launch
+ 4
− 7
View file @ c5152e43
<?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>
franka_example_controllers/launch/joint_velocity_example_controller.launch
+ 4
− 7
View file @ c5152e43
<?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>
franka_example_controllers/launch/model_example_controller.launch
+ 4
− 7
View file @ c5152e43
<?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>
franka_example_controllers/launch/rviz/franka_description_with_marker.rviz
+ 1
− 1
View file @ c5152e43
......@@ -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:
......
franka_example_controllers/scripts/interactive_marker.py
+ 6
− 6
View file @ c5152e43
......@@ -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()
......
franka_example_controllers/src/joint_position_example_controller.cpp
+ 2
− 3
View file @ c5152e43
......@@ -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) {
......
franka_example_controllers/src/joint_velocity_example_controller.cpp
+ 3
− 10
View file @ c5152e43
......@@ -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++) {
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment