diff --git a/CHANGELOG.md b/CHANGELOG.md
index f1723150187846044a768f8c333735207620fd9a..0dcc9fc9b15237e32140f946f50976c49d5e267a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,19 @@
# 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
Requires `libfranka` >= 0.3.0
diff --git a/franka_control/config/default_controllers.yaml b/franka_control/config/default_controllers.yaml
index e3a3f947df83346b42516b20258874c1a1a6e33e..533a31c51359428a4a14f797bd2db5e52640befd 100644
--- a/franka_control/config/default_controllers.yaml
+++ b/franka_control/config/default_controllers.yaml
@@ -63,4 +63,12 @@ effort_joint_trajectory_controller:
franka_state_controller:
type: franka_control/FrankaStateController
publish_rate: 30 # [Hz]
-
+ joint_names:
+ - panda_joint1
+ - panda_joint2
+ - panda_joint3
+ - panda_joint4
+ - panda_joint5
+ - panda_joint6
+ - panda_joint7
+ arm_id: panda
diff --git a/franka_control/config/franka_control.yaml b/franka_control/config/franka_control_node.yaml
similarity index 100%
rename from franka_control/config/franka_control.yaml
rename to franka_control/config/franka_control_node.yaml
diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch
index 1b2fe028515f3a31e33037665c0e09f0020c99ae..dbc6d192af7060650caf43b86b4ea162874d6af6 100644
--- a/franka_control/launch/franka_control.launch
+++ b/franka_control/launch/franka_control.launch
@@ -1,26 +1,22 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" default="robot.franka.de" />
- <arg name="arm_id" default="panda" />
<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_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)" />
+
+ <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)" />
- <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.urdf.xacro'" unless="$(arg load_gripper)" />
</node>
- <group ns="$(arg arm_id)">
- <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="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">
- <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_node/joint_states] </rosparam>
- <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
- <param name="rate" value="30"/>
- </node>
- </group>
+ <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="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">
+ <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
+ <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
+ <param name="rate" value="30"/>
+ </node>
</launch>
diff --git a/franka_control/src/franka_control_node.cpp b/franka_control/src/franka_control_node.cpp
index ac442663b4df70f9eaca462c97497494dce961ae..619268609fccf426869b47a859799e8d18e84a5b 100644
--- a/franka_control/src/franka_control_node.cpp
+++ b/franka_control/src/franka_control_node.cpp
@@ -30,7 +30,8 @@ class ServiceContainer {
};
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("~");
std::vector<std::string> joint_names_vector;
@@ -42,9 +43,16 @@ int main(int argc, char** argv) {
std::copy(joint_names_vector.cbegin(), joint_names_vector.cend(), joint_names.begin());
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;
- 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);
// Set default collision behavior
@@ -87,6 +95,7 @@ int main(int argc, char** argv) {
robot.automaticErrorRecovery();
has_error = false;
recovery_action_server.setSucceeded();
+ ROS_INFO("Recovered from error");
} catch (const franka::Exception& ex) {
recovery_action_server.setAborted(franka_control::ErrorRecoveryResult(), ex.what());
}
@@ -94,12 +103,12 @@ int main(int argc, char** argv) {
false);
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
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();
diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp
index 2faa5aea54f6cf4c66a145cc0c2b844de04f4ef1..f7af454c738370112b3e2c8adea005f62b1666ab 100644
--- a/franka_control/src/franka_state_controller.cpp
+++ b/franka_control/src/franka_state_controller.cpp
@@ -151,7 +151,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
ROS_ERROR("FrankaStateController: Could not get Franka state interface from hardware");
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");
return false;
}
@@ -163,7 +163,7 @@ bool FrankaStateController::init(hardware_interface::RobotHW* robot_hardware,
<< 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()) {
ROS_ERROR(
"FrankaStateController: Invalid or no joint_names parameters provided, aborting "
diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 62e4554ceb8e3f740a22e41b2512a28d87579698..1cd3777e10cb268a4a9d0c6998b9cefccabb30b8 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -51,14 +51,15 @@
<child link="${ns}_leftfinger"/>
<origin xyz="0 0 0.0584" rpy="0 0 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 name="${ns}_finger_joint2" type="prismatic">
<parent link="${ns}_hand"/>
<child link="${ns}_rightfinger"/>
<origin xyz="0 0 0.0584" rpy="0 0 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>
</xacro:macro>
</robot>
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/cartesian_pose_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
index e66503a5443fc7f073710ccf9e2ba39c51b93107..e712c7a88c301a531ac214ecb09ec5ad7ccab831 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h
@@ -20,9 +20,7 @@ class CartesianPoseExampleController
: public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
franka_hw::FrankaStateInterface> {
public:
- bool init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /* controller_node_handle */) override;
+ bool init(hardware_interface::RobotHW* robot_hardware, 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/cartesian_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
index 15d4970b63fb46757908e4d96aaa06dfd6c588dd..95d4ace9a9e438d98e684fc1716b86487e793b17 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h
@@ -18,9 +18,7 @@ class CartesianVelocityExampleController : public controller_interface::MultiInt
franka_hw::FrankaVelocityCartesianInterface,
franka_hw::FrankaStateInterface> {
public:
- bool init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /* controller_node_handle */) override;
+ bool init(hardware_interface::RobotHW* robot_hardware, 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/include/franka_example_controllers/joint_position_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
index daf4c4d81cb7d66ee7f4412d4202e4839452fa9f..414885106b372223a8b43b951668c5d47efc3271 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
@@ -17,9 +17,7 @@ namespace franka_example_controllers {
class JointPositionExampleController : public controller_interface::MultiInterfaceController<
hardware_interface::PositionJointInterface> {
public:
- bool init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /* controller_node_handle */) override;
+ bool init(hardware_interface::RobotHW* robot_hardware, 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..3c20999d8578a5bf1b1d073bedf24d95d3bbf08e 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
@@ -18,9 +18,7 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa
hardware_interface::VelocityJointInterface,
franka_hw::FrankaStateInterface> {
public:
- bool init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /* controller_node_handle */) override;
+ bool init(hardware_interface::RobotHW* robot_hardware, 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/move_to_start.launch b/franka_example_controllers/launch/move_to_start.launch
index ed8accb2b7fd8bc9d4e7636b08fc210e5c2f7601..c585b47c8582ab3452f188a5b27277aa97e9c9cc 100644
--- a/franka_example_controllers/launch/move_to_start.launch
+++ b/franka_example_controllers/launch/move_to_start.launch
@@ -1,18 +1,13 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" default="robot.franka.de" />
- <arg name="arm_id" default="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)" />
+ <arg name="load_gripper" value="false" />
</include>
<include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
- <arg name="arm_id" value="$(arg arm_id)" />
+ <arg name="load_gripper" value="false" />
</include>
- <group ns="$(arg arm_id)">
- <node name="move_to_start" pkg="franka_example_controllers" type="move_to_start.py" output="screen" required="true" />
- </group>
+ <node name="move_to_start" pkg="franka_example_controllers" type="move_to_start.py" output="screen" required="true" />
</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/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
index 4389fa0721909b1656eac1962cbd30b0aca9a5b1..ae1b787aa97fe89d8ce80a028a02a698600b95d4 100644
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
@@ -15,8 +15,7 @@
namespace franka_example_controllers {
bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /* controller_node_handle */) {
+ ros::NodeHandle& node_handle) {
cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
if (cartesian_pose_interface_ == nullptr) {
ROS_ERROR(
@@ -26,7 +25,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
}
std::string arm_id;
- if (!root_node_handle.getParam("arm_id", arm_id)) {
+ if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
return false;
}
diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
index ad7b64bd87f9d24f5f736f8791c5aa390ac17c94..59e0f6fedbf02a3d85d5884e74c36780694c8d95 100644
--- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
@@ -15,10 +15,9 @@
namespace franka_example_controllers {
bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
- ros::NodeHandle& root_node_handle,
- ros::NodeHandle& /* controller_node_handle */) {
+ ros::NodeHandle& node_handle) {
std::string arm_id;
- if (!root_node_handle.getParam("arm_id", arm_id)) {
+ if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("CartesianVelocityExampleController: Could not get parameter arm_id");
return false;
}
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++) {
diff --git a/franka_gripper/config/gripper_node_config.yaml b/franka_gripper/config/franka_gripper_node.yaml
similarity index 88%
rename from franka_gripper/config/gripper_node_config.yaml
rename to franka_gripper/config/franka_gripper_node.yaml
index 8799cbd639f4716f07d550162945c3a3bda08d7e..195ec8f399bacbcdf43994ab03b5d9bc8694a023 100644
--- a/franka_gripper/config/gripper_node_config.yaml
+++ b/franka_gripper/config/franka_gripper_node.yaml
@@ -5,4 +5,4 @@ joint_names:
default_speed: 0.1 # [m/s]
default_grasp_epsilon:
inner: 0.005 # [m]
- outer: 0.005 # [m]
\ No newline at end of file
+ outer: 0.005 # [m]
diff --git a/franka_gripper/include/franka_gripper/franka_gripper.h b/franka_gripper/include/franka_gripper/franka_gripper.h
index 6828577e497d2b5a7e7ab668cd8ef559c9839592..305d0698993bded6c2a78604f26ef93adc62052d 100644
--- a/franka_gripper/include/franka_gripper/franka_gripper.h
+++ b/franka_gripper/include/franka_gripper/franka_gripper.h
@@ -33,6 +33,10 @@ bool updateGripperState(const franka::Gripper& gripper, franka::GripperState* st
* Wraps the execution of a gripper command action to catch exceptions and
* report results
*
+ * @note
+ * For compatibility with current MoveIt! behavior, the given goal's command position is
+ * multiplied by a factor of 2 before being commanded to the gripper!
+ *
* @param[in] gripper A pointer to a franka gripper
* @param[in] default_speed The default speed for a gripper action
* @param[in] grasp_epsilon The epsilon window of the grasp.
diff --git a/franka_gripper/launch/franka_gripper.launch b/franka_gripper/launch/franka_gripper.launch
index d990574d4c8511c770b9612fe60a074a25fbb1ad..93c9e43474a0e62e209b28b0c1bf24e2e8182552 100644
--- a/franka_gripper/launch/franka_gripper.launch
+++ b/franka_gripper/launch/franka_gripper.launch
@@ -1,11 +1,9 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" default="robot.franka.de" />
- <arg name="arm_id" default="panda" />
- <group ns="$(arg arm_id)">
- <node name="franka_gripper_node" pkg="franka_gripper" type="franka_gripper_node" output="screen">
- <param name="robot_ip" value="$(arg robot_ip)"/>
- <rosparam command="load" file="$(find franka_gripper)/config/gripper_node_config.yaml" />
- </node>
- </group>
+
+ <node name="franka_gripper" pkg="franka_gripper" type="franka_gripper_node" output="screen">
+ <param name="robot_ip" value="$(arg robot_ip)"/>
+ <rosparam command="load" file="$(find franka_gripper)/config/franka_gripper_node.yaml" />
+ </node>
</launch>
diff --git a/franka_gripper/src/franka_gripper.cpp b/franka_gripper/src/franka_gripper.cpp
index a58493dc880d3e608dc1a50870c44df36e954e2c..53fc6d124f34257a8a7bcb532c04ee83beda1ba5 100644
--- a/franka_gripper/src/franka_gripper.cpp
+++ b/franka_gripper/src/franka_gripper.cpp
@@ -33,18 +33,26 @@ void gripperCommandExecuteCallback(
double default_speed,
actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server,
const control_msgs::GripperCommandGoalConstPtr& goal) {
- std::function<bool()> gripper_command_handler = [=, &gripper]() {
+ auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() {
+ // HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager
+ // only sends us the width of one finger. Multiply by 2 to get the intended width.
+ double target_width = 2 * goal->command.position;
+
franka::GripperState state = gripper.readOnce();
- if (goal->command.position > state.max_width || goal->command.position < 0.0) {
+ if (target_width > state.max_width || target_width < 0.0) {
ROS_ERROR_STREAM("GripperServer: Commanding out of range width! max_width = "
- << state.max_width << " command = " << goal->command.position);
+ << state.max_width << " command = " << target_width);
return false;
}
- if (goal->command.position >= state.width) {
- return gripper.move(goal->command.position, default_speed);
+ constexpr double kSamePositionThreshold = 1e-4;
+ if (std::abs(target_width - state.width) < kSamePositionThreshold) {
+ return true;
+ }
+ if (target_width >= state.width) {
+ return gripper.move(target_width, default_speed);
}
- return gripper.grasp(goal->command.position, default_speed, goal->command.max_effort,
- grasp_epsilon.inner, grasp_epsilon.outer);
+ return gripper.grasp(target_width, default_speed, goal->command.max_effort, grasp_epsilon.inner,
+ grasp_epsilon.outer);
};
try {
diff --git a/franka_visualization/launch/franka_visualization.launch b/franka_visualization/launch/franka_visualization.launch
index a713f4fd5963bbe88cad4a795130220b08ab10d0..4bbee5d6703fb830bd89efbbcaf053cc486134dc 100644
--- a/franka_visualization/launch/franka_visualization.launch
+++ b/franka_visualization/launch/franka_visualization.launch
@@ -21,8 +21,6 @@
</node>
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
- <param if="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" />
- <param unless="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" />
<param name="rate" value="$(arg publish_rate)" />
<rosparam param="source_list">[robot_joint_state_publisher/joint_states, gripper_joint_state_publisher/joint_states]</rosparam>
</node>
diff --git a/panda_moveit_config/.setup_assistant b/panda_moveit_config/.setup_assistant
index 2388fcc6116498156258c80c999a4334e7ccd1d2..8e1392be649cbbedb5f7380535b117bc68b4cca9 100644
--- a/panda_moveit_config/.setup_assistant
+++ b/panda_moveit_config/.setup_assistant
@@ -3,7 +3,7 @@ moveit_setup_assistant_config:
package: franka_description
relative_path: robots/panda_arm_hand.urdf.xacro
SRDF:
- relative_path: config/panda_arm_hand.srdf
+ relative_path: config/panda_arm_hand.srdf.xacro
CONFIG:
author_name: Franka Emika GmbH
author_email: info@franka.de
diff --git a/panda_moveit_config/config/fake_controllers.yaml b/panda_moveit_config/config/fake_controllers.yaml
index 34a8e375c1612baea1eb4189e63048e7f8442910..619aa5d08f94e9411bcef80f89d6e6a01dae1882 100644
--- a/panda_moveit_config/config/fake_controllers.yaml
+++ b/panda_moveit_config/config/fake_controllers.yaml
@@ -1,14 +1,5 @@
controller_list:
- - name: fake_panda_controller
- joints:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
- - name: fake_panda_arm_hand_controller
+ - name: fake_panda_arm_controller
joints:
- panda_joint1
- panda_joint2
@@ -21,3 +12,6 @@ controller_list:
joints:
- panda_finger_joint1
- panda_finger_joint2
+initial:
+ - group: panda_arm_hand
+ pose: ready
diff --git a/panda_moveit_config/config/hand.xacro b/panda_moveit_config/config/hand.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..566f1c1b6ddec3569864fd1067f8f7c4e510064b
--- /dev/null
+++ b/panda_moveit_config/config/hand.xacro
@@ -0,0 +1,23 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+ <xacro:macro name="hand">
+ <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+ <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+ <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+ <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+ <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+ <group name="hand">
+ <link name="panda_hand" />
+ <link name="panda_leftfinger" />
+ <link name="panda_rightfinger" />
+ <joint name="panda_finger_joint1" />
+ <passive_joint name="panda_finger_joint2" />
+ </group>
+ <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+ <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+ <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+ <disable_collisions link1="panda_hand" link2="panda_leftfinger" reason="Adjacent" />
+ <disable_collisions link1="panda_hand" link2="panda_rightfinger" reason="Adjacent" />
+ <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Default" />
+ </xacro:macro>
+</robot>
diff --git a/panda_moveit_config/config/joint_limits.yaml b/panda_moveit_config/config/joint_limits.yaml
index 43237845670eb47148ea238bd589988294a4d782..603067f270c9c54f0fc84d419cbad4ca022b2979 100644
--- a/panda_moveit_config/config/joint_limits.yaml
+++ b/panda_moveit_config/config/joint_limits.yaml
@@ -2,48 +2,48 @@
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
- panda_finger_joint1:
- has_velocity_limits: true
- max_velocity: 0.3
- has_acceleration_limits: true
- max_acceleration: 0.5
- panda_finger_joint2:
- has_velocity_limits: true
- max_velocity: 0.3
- has_acceleration_limits: true
- max_acceleration: 0.5
panda_joint1:
has_velocity_limits: true
- max_velocity: 0.5
- has_acceleration_limits: true
- max_acceleration: 0.5
+ max_velocity: 2.3925
+ has_acceleration_limits: false
+ max_acceleration: 0
panda_joint2:
has_velocity_limits: true
- max_velocity: 0.5
- has_acceleration_limits: true
- max_acceleration: 0.5
+ max_velocity: 2.3925
+ has_acceleration_limits: false
+ max_acceleration: 0
panda_joint3:
has_velocity_limits: true
- max_velocity: 0.5
- has_acceleration_limits: true
- max_acceleration: 0.5
+ max_velocity: 2.3925
+ has_acceleration_limits: false
+ max_acceleration: 0
panda_joint4:
has_velocity_limits: true
- max_velocity: 0.5
- has_acceleration_limits: true
- max_acceleration: 0.5
+ max_velocity: 2.3925
+ has_acceleration_limits: false
+ max_acceleration: 0
panda_joint5:
has_velocity_limits: true
- max_velocity: 0.5
- has_acceleration_limits: true
- max_acceleration: 0.5
+ max_velocity: 2.871
+ has_acceleration_limits: false
+ max_acceleration: 0
panda_joint6:
has_velocity_limits: true
- max_velocity: 0.5
- has_acceleration_limits: true
- max_acceleration: 0.5
+ max_velocity: 2.871
+ has_acceleration_limits: false
+ max_acceleration: 0
panda_joint7:
has_velocity_limits: true
- max_velocity: 0.5
- has_acceleration_limits: true
- max_acceleration: 0.5
+ max_velocity: 2.871
+ has_acceleration_limits: false
+ max_acceleration: 0
+ panda_finger_joint1:
+ has_velocity_limits: true
+ max_velocity: 0.1
+ has_acceleration_limits: false
+ max_acceleration: 0
+ panda_finger_joint2:
+ has_velocity_limits: true
+ max_velocity: 0.1
+ has_acceleration_limits: false
+ max_acceleration: 0
diff --git a/panda_moveit_config/config/kinematics.yaml b/panda_moveit_config/config/kinematics.yaml
index c64ffe30854d552745a9b49237cd6d7d50fa567e..7ec1735f3ec4ebfa578f2f7b9b785877e724f8a3 100644
--- a/panda_moveit_config/config/kinematics.yaml
+++ b/panda_moveit_config/config/kinematics.yaml
@@ -1,9 +1,4 @@
-panda:
- kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
- kinematics_solver_search_resolution: 0.005
- kinematics_solver_timeout: 0.005
- kinematics_solver_attempts: 3
-panda_arm_hand:
+panda_arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
diff --git a/panda_moveit_config/config/ompl_planning.yaml b/panda_moveit_config/config/ompl_planning.yaml
index 9251f54c717d95c756e281cf95f9363f7ba6f5cc..17d4c5aca58d8d958768dccb3690f125879ac8e9 100644
--- a/panda_moveit_config/config/ompl_planning.yaml
+++ b/panda_moveit_config/config/ompl_planning.yaml
@@ -120,7 +120,7 @@ planner_configs:
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
-panda:
+panda_arm:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
diff --git a/panda_moveit_config/config/panda_arm.srdf.xacro b/panda_moveit_config/config/panda_arm.srdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..76b4c9ee266915df3be1c8c4f3fd48a8972b6144
--- /dev/null
+++ b/panda_moveit_config/config/panda_arm.srdf.xacro
@@ -0,0 +1,9 @@
+<?xml version='1.0' encoding='utf-8'?>
+<!--This does not replace URDF, and is not an extension of URDF.
+ This is a format for representing semantic information about the robot structure.
+ A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+ <xacro:include filename="$(find panda_moveit_config)/config/panda_arm.xacro" />
+ <xacro:panda_arm />
+</robot>
diff --git a/panda_moveit_config/config/panda_arm.xacro b/panda_moveit_config/config/panda_arm.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..bb1683520e1357a35d0d3aaea01a11ed6d284d7d
--- /dev/null
+++ b/panda_moveit_config/config/panda_arm.xacro
@@ -0,0 +1,62 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+ <xacro:macro name="panda_arm">
+ <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+ <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+ <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+ <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+ <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+ <group name="panda_arm">
+ <link name="panda_link0" />
+ <link name="panda_link1" />
+ <link name="panda_link2" />
+ <link name="panda_link3" />
+ <link name="panda_link4" />
+ <link name="panda_link5" />
+ <link name="panda_link6" />
+ <link name="panda_link7" />
+ <link name="panda_link8" />
+ <joint name="panda_joint1" />
+ <joint name="panda_joint2" />
+ <joint name="panda_joint3" />
+ <joint name="panda_joint4" />
+ <joint name="panda_joint5" />
+ <joint name="panda_joint6" />
+ <joint name="panda_joint7" />
+ <joint name="panda_joint8" />
+ <chain base_link="panda_link0" tip_link="panda_link8" />
+ </group>
+ <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+ <group_state name="ready" group="panda_arm">
+ <joint name="panda_joint1" value="0" />
+ <joint name="panda_joint2" value="-0.785" />
+ <joint name="panda_joint3" value="0" />
+ <joint name="panda_joint4" value="-2.356" />
+ <joint name="panda_joint5" value="0" />
+ <joint name="panda_joint6" value="1.571" />
+ <joint name="panda_joint7" value="0.785" />
+ </group_state>
+ <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+ <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+ <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
+ <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />
+ <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never" />
+ <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never" />
+ <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent" />
+ <disable_collisions link1="panda_link1" link2="panda_link3" reason="Never" />
+ <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never" />
+ <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent" />
+ <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never" />
+ <disable_collisions link1="panda_link2" link2="panda_link6" reason="Never" />
+ <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent" />
+ <disable_collisions link1="panda_link3" link2="panda_link5" reason="Never" />
+ <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never" />
+ <disable_collisions link1="panda_link3" link2="panda_link7" reason="Never" />
+ <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent" />
+ <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never" />
+ <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never" />
+ <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
+ <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
+ <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
+ </xacro:macro>
+</robot>
diff --git a/panda_moveit_config/config/panda_arm_hand.srdf b/panda_moveit_config/config/panda_arm_hand.srdf
deleted file mode 100644
index 56981c799cc7cd62acc75184f627599bcb5cf70e..0000000000000000000000000000000000000000
--- a/panda_moveit_config/config/panda_arm_hand.srdf
+++ /dev/null
@@ -1,137 +0,0 @@
-<?xml version="1.0" ?>
-<!--This does not replace URDF, and is not an extension of URDF.
- This is a format for representing semantic information about the robot structure.
- A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
--->
-<robot name="panda">
- <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
- <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
- <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
- <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
- <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
- <group name="panda_arm">
- <link name="panda_link0" />
- <link name="panda_link1" />
- <link name="panda_link2" />
- <link name="panda_link3" />
- <link name="panda_link4" />
- <link name="panda_link5" />
- <link name="panda_link6" />
- <link name="panda_link7" />
- <link name="panda_link8" />
- <joint name="panda_joint1" />
- <joint name="panda_joint2" />
- <joint name="panda_joint3" />
- <joint name="panda_joint4" />
- <joint name="panda_joint5" />
- <joint name="panda_joint6" />
- <joint name="panda_joint7" />
- <joint name="panda_joint8" />
- <chain base_link="panda_link0" tip_link="panda_link8" />
- </group>
- <group name="panda_arm_hand">
- <link name="panda_link0" />
- <link name="panda_link1" />
- <link name="panda_link2" />
- <link name="panda_link3" />
- <link name="panda_link4" />
- <link name="panda_link5" />
- <link name="panda_link6" />
- <link name="panda_link7" />
- <link name="panda_link8" />
- <link name="panda_hand" />
- <joint name="panda_joint1" />
- <joint name="panda_joint2" />
- <joint name="panda_joint3" />
- <joint name="panda_joint4" />
- <joint name="panda_joint5" />
- <joint name="panda_joint6" />
- <joint name="panda_joint7" />
- <joint name="panda_joint8" />
- <joint name="panda_hand_joint" />
- <chain base_link="panda_link0" tip_link="panda_hand" />
- </group>
- <group name="hand">
- <link name="panda_hand" />
- <link name="panda_leftfinger" />
- <link name="panda_rightfinger" />
- <joint name="panda_finger_joint1" />
- <joint name="panda_finger_joint2" />
- </group>
- <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
- <group_state name="home" group="panda_arm">
- <joint name="panda_joint1" value="0" />
- <joint name="panda_joint2" value="0" />
- <joint name="panda_joint3" value="0" />
- <joint name="panda_joint4" value="0" />
- <joint name="panda_joint5" value="0" />
- <joint name="panda_joint6" value="0" />
- <joint name="panda_joint7" value="0.785" />
- </group_state>
- <group_state name="ready" group="panda_arm">
- <joint name="panda_joint1" value="0" />
- <joint name="panda_joint2" value="-0.785" />
- <joint name="panda_joint3" value="0" />
- <joint name="panda_joint4" value="-2.356" />
- <joint name="panda_joint5" value="0" />
- <joint name="panda_joint6" value="1.571" />
- <joint name="panda_joint7" value="0.785" />
- </group_state>
- <group_state name="home" group="panda_arm_hand">
- <joint name="panda_joint1" value="0" />
- <joint name="panda_joint2" value="0" />
- <joint name="panda_joint3" value="0" />
- <joint name="panda_joint4" value="0" />
- <joint name="panda_joint5" value="0" />
- <joint name="panda_joint6" value="0" />
- <joint name="panda_joint7" value="0.785" />
- </group_state>
- <group_state name="ready" group="panda_arm_hand">
- <joint name="panda_joint1" value="0" />
- <joint name="panda_joint2" value="-0.785" />
- <joint name="panda_joint3" value="0" />
- <joint name="panda_joint4" value="-2.356" />
- <joint name="panda_joint5" value="0" />
- <joint name="panda_joint6" value="1.571" />
- <joint name="panda_joint7" value="0.785" />
- </group_state>
- <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
- <end_effector name="hand" parent_link="panda_hand" group="hand" parent_group="panda_arm_hand" />
- <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
- <disable_collisions link1="panda_hand" link2="panda_leftfinger" reason="Adjacent" />
- <disable_collisions link1="panda_hand" link2="panda_link3" reason="Never" />
- <disable_collisions link1="panda_hand" link2="panda_link4" reason="Never" />
- <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default" />
- <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never" />
- <disable_collisions link1="panda_hand" link2="panda_link7" reason="Adjacent" />
- <disable_collisions link1="panda_hand" link2="panda_rightfinger" reason="Adjacent" />
- <disable_collisions link1="panda_leftfinger" link2="panda_link3" reason="Never" />
- <disable_collisions link1="panda_leftfinger" link2="panda_link4" reason="Never" />
- <disable_collisions link1="panda_leftfinger" link2="panda_link6" reason="Never" />
- <disable_collisions link1="panda_leftfinger" link2="panda_link7" reason="Never" />
- <disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Default" />
- <disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent" />
- <disable_collisions link1="panda_link0" link2="panda_link2" reason="Never" />
- <disable_collisions link1="panda_link0" link2="panda_link3" reason="Never" />
- <disable_collisions link1="panda_link0" link2="panda_link4" reason="Never" />
- <disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent" />
- <disable_collisions link1="panda_link1" link2="panda_link3" reason="Never" />
- <disable_collisions link1="panda_link1" link2="panda_link4" reason="Never" />
- <disable_collisions link1="panda_link2" link2="panda_link3" reason="Adjacent" />
- <disable_collisions link1="panda_link2" link2="panda_link4" reason="Never" />
- <disable_collisions link1="panda_link2" link2="panda_link6" reason="Never" />
- <disable_collisions link1="panda_link3" link2="panda_link4" reason="Adjacent" />
- <disable_collisions link1="panda_link3" link2="panda_link5" reason="Never" />
- <disable_collisions link1="panda_link3" link2="panda_link6" reason="Never" />
- <disable_collisions link1="panda_link3" link2="panda_link7" reason="Never" />
- <disable_collisions link1="panda_link3" link2="panda_rightfinger" reason="Never" />
- <disable_collisions link1="panda_link4" link2="panda_link5" reason="Adjacent" />
- <disable_collisions link1="panda_link4" link2="panda_link6" reason="Never" />
- <disable_collisions link1="panda_link4" link2="panda_link7" reason="Never" />
- <disable_collisions link1="panda_link4" link2="panda_rightfinger" reason="Never" />
- <disable_collisions link1="panda_link5" link2="panda_link6" reason="Adjacent" />
- <disable_collisions link1="panda_link5" link2="panda_link7" reason="Default" />
- <disable_collisions link1="panda_link6" link2="panda_link7" reason="Adjacent" />
- <disable_collisions link1="panda_link6" link2="panda_rightfinger" reason="Never" />
- <disable_collisions link1="panda_link7" link2="panda_rightfinger" reason="Never" />
-</robot>
diff --git a/panda_moveit_config/config/panda_arm_hand.srdf.xacro b/panda_moveit_config/config/panda_arm_hand.srdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..df3985bbd1a3ca0d4aaf23740733123b02188a88
--- /dev/null
+++ b/panda_moveit_config/config/panda_arm_hand.srdf.xacro
@@ -0,0 +1,46 @@
+<?xml version='1.0' encoding='utf-8'?>
+<!--This does not replace URDF, and is not an extension of URDF.
+ This is a format for representing semantic information about the robot structure.
+ A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+ <xacro:include filename="$(find panda_moveit_config)/config/panda_arm.xacro" />
+ <xacro:include filename="$(find panda_moveit_config)/config/hand.xacro" />
+ <xacro:panda_arm />
+ <xacro:hand />
+ <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+ <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+ <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+ <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+ <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+ <group name="panda_arm_hand">
+ <group name="panda_arm" />
+ <group name="hand" />
+ </group>
+ <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+ <group_state name="ready" group="panda_arm_hand">
+ <joint name="panda_joint1" value="0" />
+ <joint name="panda_joint2" value="-0.785" />
+ <joint name="panda_joint3" value="0" />
+ <joint name="panda_joint4" value="-2.356" />
+ <joint name="panda_joint5" value="0" />
+ <joint name="panda_joint6" value="1.571" />
+ <joint name="panda_joint7" value="0.785" />
+ </group_state>
+ <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+ <end_effector name="hand" parent_link="panda_hand" group="hand" parent_group="panda_arm_hand" />
+ <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+ <disable_collisions link1="panda_hand" link2="panda_link3" reason="Never" />
+ <disable_collisions link1="panda_hand" link2="panda_link4" reason="Never" />
+ <disable_collisions link1="panda_hand" link2="panda_link5" reason="Default" />
+ <disable_collisions link1="panda_hand" link2="panda_link6" reason="Never" />
+ <disable_collisions link1="panda_hand" link2="panda_link7" reason="Adjacent" />
+ <disable_collisions link1="panda_leftfinger" link2="panda_link3" reason="Never" />
+ <disable_collisions link1="panda_leftfinger" link2="panda_link4" reason="Never" />
+ <disable_collisions link1="panda_leftfinger" link2="panda_link6" reason="Never" />
+ <disable_collisions link1="panda_leftfinger" link2="panda_link7" reason="Never" />
+ <disable_collisions link1="panda_link3" link2="panda_rightfinger" reason="Never" />
+ <disable_collisions link1="panda_link4" link2="panda_rightfinger" reason="Never" />
+ <disable_collisions link1="panda_link6" link2="panda_rightfinger" reason="Never" />
+ <disable_collisions link1="panda_link7" link2="panda_rightfinger" reason="Never" />
+</robot>
diff --git a/panda_moveit_config/config/panda_controllers.yaml b/panda_moveit_config/config/panda_controllers.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..29d83ea46a8576121aaa76251741966411a56c2f
--- /dev/null
+++ b/panda_moveit_config/config/panda_controllers.yaml
@@ -0,0 +1,12 @@
+controller_list:
+ - name: position_joint_trajectory_controller
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ joints:
+ - panda_joint1
+ - panda_joint2
+ - panda_joint3
+ - panda_joint4
+ - panda_joint5
+ - panda_joint6
+ - panda_joint7
diff --git a/panda_moveit_config/config/panda_gripper_controllers.yaml b/panda_moveit_config/config/panda_gripper_controllers.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1b2318485469afbf32bddb82438484d842d8bc94
--- /dev/null
+++ b/panda_moveit_config/config/panda_gripper_controllers.yaml
@@ -0,0 +1,18 @@
+controller_list:
+ - name: position_joint_trajectory_controller
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ joints:
+ - panda_joint1
+ - panda_joint2
+ - panda_joint3
+ - panda_joint4
+ - panda_joint5
+ - panda_joint6
+ - panda_joint7
+ - name: franka_gripper
+ action_ns: gripper_action
+ type: GripperCommand
+ joints:
+ - panda_finger_joint1
+ - panda_finger_joint2
diff --git a/panda_moveit_config/launch/move_group.launch b/panda_moveit_config/launch/move_group.launch
index 5b8c8665ad9d5fcd871f8dc43a42c27875a232a4..d5ef479501923c14ce9036c6f3433d6fd07d8844 100644
--- a/panda_moveit_config/launch/move_group.launch
+++ b/panda_moveit_config/launch/move_group.launch
@@ -1,6 +1,10 @@
<launch>
- <include file="$(find panda_moveit_config)/launch/planning_context.launch" />
+ <arg name="load_gripper" default="true" />
+
+ <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+ <arg name="load_gripper" value="$(arg load_gripper)" />
+ </include>
<!-- GDB Debug Option -->
<arg name="debug" default="false" />
@@ -27,7 +31,8 @@
<!-- Trajectory Execution Functionality -->
<include ns="move_group" file="$(find panda_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
- <arg name="moveit_controller_manager" value="panda" unless="$(arg fake_execution)"/>
+ <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
+ <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
</include>
diff --git a/panda_moveit_config/launch/moveit_rviz.launch b/panda_moveit_config/launch/moveit_rviz.launch
index 71f196efab7eb3e525978ab35d8d1f31a0d4e018..17387f9e2923ca48cf2cb8feab98e92c0e297491 100644
--- a/panda_moveit_config/launch/moveit_rviz.launch
+++ b/panda_moveit_config/launch/moveit_rviz.launch
@@ -8,13 +8,9 @@
<arg unless="$(arg config)" name="command_args" value="" />
<arg if="$(arg config)" name="command_args" value="-d $(find panda_moveit_config)/launch/moveit.rviz" />
- <arg name="arm_id" default="panda" />
-
- <group ns="$(arg arm_id)">
- <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
- args="$(arg command_args)" output="screen">
- <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
- </node>
- </group>
+ <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
+ args="$(arg command_args)" output="screen">
+ <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+ </node>
</launch>
diff --git a/panda_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml b/panda_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000000000000000000000000000000000000..75a68a0d8bd7bfef00e4311edfb0680701a35db0
--- /dev/null
+++ b/panda_moveit_config/launch/panda_gripper_moveit_controller_manager.launch.xml
@@ -0,0 +1,6 @@
+<launch>
+ <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+ <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+
+ <rosparam file="$(find panda_moveit_config)/config/panda_gripper_controllers.yaml"/>
+</launch>
diff --git a/panda_moveit_config/launch/panda_moveit.launch b/panda_moveit_config/launch/panda_moveit.launch
index a879467f3b7a9e5122e095ccf36f14231585e8da..dd5c0add4ce04104570861e242a17d17213e52dd 100644
--- a/panda_moveit_config/launch/panda_moveit.launch
+++ b/panda_moveit_config/launch/panda_moveit.launch
@@ -1,11 +1,12 @@
<?xml version="1.0" ?>
<launch>
- <arg name="arm_id" default="panda" />
<!-- Valid values: "position", "effort" -->
<arg name="controller" default="position" />
+ <arg name="load_gripper" default="true" />
- <group ns="$(arg arm_id)">
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)_joint_trajectory_controller"/>
- <include file="$(find panda_moveit_config)/launch/move_group.launch" />
- </group>
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="position_joint_trajectory_controller"/>
+
+ <include file="$(find panda_moveit_config)/launch/move_group.launch">
+ <arg name="load_gripper" value="$(arg load_gripper)" />
+ </include>
</launch>
diff --git a/panda_moveit_config/launch/panda_moveit_controller_manager.launch.xml b/panda_moveit_config/launch/panda_moveit_controller_manager.launch.xml
index 353670175cebaad2fcdb8b356ef235c6444943ec..afd6d37973c9209fb74ad2b9f83cc68761df22d0 100644
--- a/panda_moveit_config/launch/panda_moveit_controller_manager.launch.xml
+++ b/panda_moveit_config/launch/panda_moveit_controller_manager.launch.xml
@@ -1,4 +1,6 @@
<launch>
- <arg name="moveit_controller_manager" default="moveit_ros_control_interface::MoveItMultiControllerManager" />
- <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+ <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+ <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
+
+ <rosparam file="$(find panda_moveit_config)/config/panda_controllers.yaml"/>
</launch>
diff --git a/panda_moveit_config/launch/planning_context.launch b/panda_moveit_config/launch/planning_context.launch
index 1d4b6e048d31ab5bd3144bc324cc69945b5d90ae..e96133948929741d8d126e5942e648f3969029dd 100644
--- a/panda_moveit_config/launch/planning_context.launch
+++ b/panda_moveit_config/launch/planning_context.launch
@@ -1,4 +1,6 @@
<launch>
+ <arg name="load_gripper" default="true" />
+
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_robot_description" default="false"/>
@@ -6,10 +8,12 @@
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
- <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
+ <param if="$(eval arg('load_robot_description') and arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
+ <param if="$(eval arg('load_robot_description') and not arg('load_gripper'))" name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
<!-- The semantic description that corresponds to the URDF -->
- <param name="$(arg robot_description)_semantic" textfile="$(find panda_moveit_config)/config/panda_arm_hand.srdf" />
+ <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder '$(find panda_moveit_config)/config/panda_arm_hand.srdf.xacro'" if="$(arg load_gripper)" />
+ <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder '$(find panda_moveit_config)/config/panda_arm.srdf.xacro'" unless="$(arg load_gripper)" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
diff --git a/panda_moveit_config/launch/trajectory_execution.launch.xml b/panda_moveit_config/launch/trajectory_execution.launch.xml
index b6c03c9b75d36198aa21aecd195f6a62ad4bf96a..72906ef58f7dafd89692db66ad0c22e4f2792408 100644
--- a/panda_moveit_config/launch/trajectory_execution.launch.xml
+++ b/panda_moveit_config/launch/trajectory_execution.launch.xml
@@ -7,7 +7,7 @@
<param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
<!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
- <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
+ <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.5"/> <!-- default 1.2 -->
<!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
<param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
<!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->