Skip to content
Snippets Groups Projects
Unverified Commit 81687d05 authored by Florian Walch's avatar Florian Walch Committed by GitHub
Browse files

Merge pull request #13 from fwalch/SWDEV-561-moveit-fixes

panda_moveit_config: Fixes for demo.launch, gripper support
parents d29d459e 4bbcd60a
Branches
Tags
No related merge requests found
Showing
with 115 additions and 99 deletions
# CHANGELOG # CHANGELOG
## 0.4.0 - UNRELEASED
* **BREAKING** Removed `arm_id` from launchfiles
* **BREAKING** Changed namespace of `franka_control` controller manager
* **BREAKING** Changed behavior of `gripper_action` for compatibility with MoveIt
* Changes in `panda_moveit_config`:
* Updated joint limits from URDF
* Removed `home` poses
* Fixed fake execution
* Add `load_gripper` argument (default: `true`) to `panda_moveit.launch`
* Conditionally load controllers/SRDFs based on `load_gripper`
* Add gripper controller configuration (requires running `franka_gripper_node`)
* Added `mimic` tag for gripper fingers to URDF and fixed velocity limits
## 0.3.0 - 2018-02-22 ## 0.3.0 - 2018-02-22
Requires `libfranka` >= 0.3.0 Requires `libfranka` >= 0.3.0
......
...@@ -63,4 +63,12 @@ effort_joint_trajectory_controller: ...@@ -63,4 +63,12 @@ effort_joint_trajectory_controller:
franka_state_controller: franka_state_controller:
type: franka_control/FrankaStateController type: franka_control/FrankaStateController
publish_rate: 30 # [Hz] publish_rate: 30 # [Hz]
joint_names:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
arm_id: panda
<?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" default="panda" />
<arg name="load_gripper" default="true" /> <arg name="load_gripper" default="true" />
<node name="$(arg arm_id)" pkg="franka_control" type="franka_control_node" output="screen" required="true">
<rosparam command="load" file="$(find franka_control)/config/franka_control.yaml" />
<param name="robot_ip" value="$(arg robot_ip)" />
<param name="arm_id" value="$(arg arm_id)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" /> <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" /> <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" />
<node name="franka_control" pkg="franka_control" type="franka_control_node" output="screen" required="true">
<rosparam command="load" file="$(find franka_control)/config/franka_control_node.yaml" />
<param name="robot_ip" value="$(arg robot_ip)" />
</node> </node>
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" /> <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
<node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/> <node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen"> <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" /> <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
<rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper_node/joint_states] </rosparam>
<rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam> <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
<param name="rate" value="30"/> <param name="rate" value="30"/>
</node> </node>
</group>
</launch> </launch>
...@@ -30,7 +30,8 @@ class ServiceContainer { ...@@ -30,7 +30,8 @@ class ServiceContainer {
}; };
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "franka_hw"); ros::init(argc, argv, "franka_control_node");
ros::NodeHandle public_node_handle;
ros::NodeHandle node_handle("~"); ros::NodeHandle node_handle("~");
std::vector<std::string> joint_names_vector; std::vector<std::string> joint_names_vector;
...@@ -42,9 +43,16 @@ int main(int argc, char** argv) { ...@@ -42,9 +43,16 @@ int main(int argc, char** argv) {
std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin()); std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
std::string robot_ip; std::string robot_ip;
node_handle.getParam("robot_ip", robot_ip); if (!node_handle.getParam("robot_ip", robot_ip)) {
ROS_ERROR("Invalid or no robot_ip parameter provided");
return 1;
}
std::string arm_id; std::string arm_id;
node_handle.getParam("arm_id", arm_id); if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("Invalid or no arm_id parameter provided");
return 1;
}
franka::Robot robot(robot_ip); franka::Robot robot(robot_ip);
// Set default collision behavior // Set default collision behavior
...@@ -87,6 +95,7 @@ int main(int argc, char** argv) { ...@@ -87,6 +95,7 @@ int main(int argc, char** argv) {
robot.automaticErrorRecovery(); robot.automaticErrorRecovery();
has_error = false; has_error = false;
recovery_action_server.setSucceeded(); recovery_action_server.setSucceeded();
ROS_INFO("Recovered from error");
} catch (const franka::Exception& ex) { } catch (const franka::Exception& ex) {
recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what()); recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what());
} }
...@@ -94,12 +103,12 @@ int main(int argc, char** argv) { ...@@ -94,12 +103,12 @@ int main(int argc, char** argv) {
false); false);
franka::Model model = robot.loadModel(); franka::Model model = robot.loadModel();
franka_hw::FrankaHW franka_control(joint_names, arm_id, node_handle, model); franka_hw::FrankaHW franka_control(joint_names, arm_id, public_node_handle, model);
// Initialize robot state before loading any controller // Initialize robot state before loading any controller
franka_control.update(robot.readOnce()); franka_control.update(robot.readOnce());
controller_manager::ControllerManager control_manager(&franka_control, node_handle); controller_manager::ControllerManager control_manager(&franka_control, public_node_handle);
recovery_action_server.start(); recovery_action_server.start();
......
...@@ -151,7 +151,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware, ...@@ -151,7 +151,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware"); ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
return false; return false;
} }
if (!root_node_handle.getParam("arm_id", arm_id_)) { if (!controller_node_handle.getParam("arm_id", arm_id_)) {
ROS_ERROR("FrankaStateController: Could not get parameter arm_id"); ROS_ERROR("FrankaStateController: Could not get parameter arm_id");
return false; return false;
} }
...@@ -163,7 +163,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware, ...@@ -163,7 +163,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
<< publish_rate << " [Hz]."); << publish_rate << " [Hz].");
} }
if (!root_node_handle.getParam("joint_names", joint_names_) || if (!controller_node_handle.getParam("joint_names", joint_names_) ||
joint_names_.size() != robot_state_.q.size()) { joint_names_.size() != robot_state_.q.size()) {
ROS_ERROR( ROS_ERROR(
"FrankaStateController: Invalid or no joint_names parameters provided, aborting " "FrankaStateController: Invalid or no joint_names parameters provided, aborting "
......
...@@ -51,14 +51,15 @@ ...@@ -51,14 +51,15 @@
<child link="${ns}_leftfinger"/> <child link="${ns}_leftfinger"/>
<origin xyz="0 0 0.0584" rpy="0 0 0"/> <origin xyz="0 0 0.0584" rpy="0 0 0"/>
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<limit effort="20" lower="-0.001" upper="0.04" velocity="0.3"/> <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
</joint> </joint>
<joint name="${ns}_finger_joint2" type="prismatic"> <joint name="${ns}_finger_joint2" type="prismatic">
<parent link="${ns}_hand"/> <parent link="${ns}_hand"/>
<child link="${ns}_rightfinger"/> <child link="${ns}_rightfinger"/>
<origin xyz="0 0 0.0584" rpy="0 0 0"/> <origin xyz="0 0 0.0584" rpy="0 0 0"/>
<axis xyz="0 -1 0"/> <axis xyz="0 -1 0"/>
<limit effort="20" lower="-0.001" upper="0.04" velocity="0.3"/> <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
<mimic joint="${ns}_finger_joint1" />
</joint> </joint>
</xacro:macro> </xacro:macro>
</robot> </robot>
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
......
...@@ -20,9 +20,7 @@ class CartesianPoseExampleController ...@@ -20,9 +20,7 @@ class CartesianPoseExampleController
: public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface, : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
franka_hw::FrankaStateInterface> { franka_hw::FrankaStateInterface> {
public: public:
bool init(hardware_interface::RobotHW* robot_hardware, bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
ros::NodeHandle& root_node_handle,
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;
......
...@@ -18,9 +18,7 @@ class CartesianVelocityExampleController : public controller_interface::MultiInt ...@@ -18,9 +18,7 @@ class CartesianVelocityExampleController : public controller_interface::MultiInt
franka_hw::FrankaVelocityCartesianInterface, franka_hw::FrankaVelocityCartesianInterface,
franka_hw::FrankaStateInterface> { franka_hw::FrankaStateInterface> {
public: public:
bool init(hardware_interface::RobotHW* robot_hardware, bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
ros::NodeHandle& root_node_handle,
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;
......
...@@ -17,9 +17,7 @@ namespace franka_example_controllers { ...@@ -17,9 +17,7 @@ namespace franka_example_controllers {
class JointPositionExampleController : public controller_interface::MultiInterfaceController< class JointPositionExampleController : public controller_interface::MultiInterfaceController<
hardware_interface::PositionJointInterface> { hardware_interface::PositionJointInterface> {
public: public:
bool init(hardware_interface::RobotHW* robot_hardware, bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
ros::NodeHandle& root_node_handle,
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;
......
...@@ -18,9 +18,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa ...@@ -18,9 +18,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa
hardware_interface::VelocityJointInterface, hardware_interface::VelocityJointInterface,
franka_hw::FrankaStateInterface> { franka_hw::FrankaStateInterface> {
public: public:
bool init(hardware_interface::RobotHW* robot_hardware, bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
ros::NodeHandle& root_node_handle,
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>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment