From 041d3eb36749cb2afdba8ed21555b306ee6dab04 Mon Sep 17 00:00:00 2001
From: Florian Walch <florian.walch@franka.de>
Date: Wed, 1 Aug 2018 13:37:48 +0200
Subject: [PATCH] Update changelog, minor fixes

---
 CHANGELOG.md                                  | 11 ++++--
 .../config/default_controllers.yaml           | 35 -------------------
 .../launch/move_to_start.launch               |  1 -
 franka_hw/src/franka_hw.cpp                   | 10 +++---
 .../launch/panda_control_moveit_rviz.launch   |  1 -
 5 files changed, 14 insertions(+), 44 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 2406a78..8a91451 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -4,8 +4,15 @@
 
 Requires `libfranka` >= 0.6.0
 
-  * Fixed joint velocity and acceleration limits for MoveIt
-  * Fixed limits in `franka_hw`
+  * **BREAKING** Updated joint limits in URDF
+  * **BREAKING** Fixed velocity, acceleration and jerk limits in `franka_hw`
+  * Allow to configure rate limiting, filtering and internal controller in `franka_control_node`
+  * **BREAKING** Enabled limiting and low-pass filtering by default (`franka_control_node.yaml`)
+  * Publish desired joint state in `/joint_state_desired`
+  * Removed `effort_joint_trajectory_controller` from `default_controllers.yaml`
+  * **BREAKING** Fixes for MoveIt:
+    * Fixed joint velocity and acceleration limits in `joint_limits.yaml`
+    * Use desired joint state for move group
 
 ## 0.5.0 - 2018-06-28
 
diff --git a/franka_control/config/default_controllers.yaml b/franka_control/config/default_controllers.yaml
index 533a31c..28b4805 100644
--- a/franka_control/config/default_controllers.yaml
+++ b/franka_control/config/default_controllers.yaml
@@ -25,41 +25,6 @@ position_joint_trajectory_controller:
     panda_joint7:
       goal: 0.05
 
-effort_joint_trajectory_controller:
-  type: effort_controllers/JointTrajectoryController
-  joints:
-    - panda_joint1
-    - panda_joint2
-    - panda_joint3
-    - panda_joint4
-    - panda_joint5
-    - panda_joint6
-    - panda_joint7
-  constraints:
-    goal_time: 0.5
-    panda_joint1:
-      goal: 0.05
-    panda_joint2:
-      goal: 0.05
-    panda_joint3:
-      goal: 0.05
-    panda_joint4:
-      goal: 0.05
-    panda_joint5:
-      goal: 0.05
-    panda_joint6:
-      goal: 0.05
-    panda_joint7:
-      goal: 0.05
-  gains:
-    panda_joint1: {p: 100, d: 10, i: 0, i_clamp: 1}
-    panda_joint2: {p: 100, d: 10, i: 0, i_clamp: 1}
-    panda_joint3: {p: 100, d: 10, i: 0, i_clamp: 1}
-    panda_joint4: {p: 100, d: 10, i: 0, i_clamp: 1}
-    panda_joint5: {p: 100, d: 10, i: 0, i_clamp: 1}
-    panda_joint6: {p: 100, d: 10, i: 0, i_clamp: 1}
-    panda_joint7: {p: 100, d: 10, i: 0, i_clamp: 1}
-
 franka_state_controller:
   type: franka_control/FrankaStateController
   publish_rate: 30  # [Hz]
diff --git a/franka_example_controllers/launch/move_to_start.launch b/franka_example_controllers/launch/move_to_start.launch
index a33d162..d27edf4 100644
--- a/franka_example_controllers/launch/move_to_start.launch
+++ b/franka_example_controllers/launch/move_to_start.launch
@@ -5,7 +5,6 @@
   <include file="$(find franka_control)/launch/franka_control.launch">
     <arg name="robot_ip" value="$(arg robot_ip)" />
     <arg name="load_gripper" value="false" />
-    <arg name="publish_desired_values" value="true" />
   </include>
   <include file="$(find panda_moveit_config)/launch/panda_moveit.launch">
     <arg name="load_gripper" value="false" />
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index 965694f..5baa805 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/franka_hw/src/franka_hw.cpp
@@ -29,7 +29,7 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
       pose_cartesian_command_(
           {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}),
       velocity_cartesian_command_({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) {
-  for (size_t i = 0; i < joint_names_.size(); ++i) {
+  for (size_t i = 0; i < joint_names_.size(); i++) {
     hardware_interface::JointStateHandle joint_handle_q_d(
         joint_names_[i], &robot_state_.q_d[i], &robot_state_.dq_d[i], &robot_state_.tau_J[i]);
 
@@ -59,8 +59,8 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
       joint_limits_interface::SoftJointLimits soft_limits;
       joint_limits_interface::JointLimits joint_limits;
 
-      for (auto joint_name : joint_names_) {
-        int joint_index(std::stoi(joint_name.substr(joint_name.size() - 1)) - 1);
+      for (size_t i = 0; i < joint_names_.size(); i++) {
+        const std::string& joint_name = joint_names_[i];
         auto urdf_joint = urdf_model.getJoint(joint_name);
         if (!urdf_joint) {
           ROS_ERROR_STREAM("FrankaHW: Could not get joint " << joint_name << " from urdf");
@@ -74,9 +74,9 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
 
         if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
           if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
-            joint_limits.max_acceleration = franka::kMaxJointAcceleration[joint_index];
+            joint_limits.max_acceleration = franka::kMaxJointAcceleration[i];
             joint_limits.has_acceleration_limits = true;
-            joint_limits.max_jerk = franka::kMaxJointJerk[joint_index];
+            joint_limits.max_jerk = franka::kMaxJointJerk[i];
             joint_limits.has_jerk_limits = true;
             joint_limits_interface::PositionJointSoftLimitsHandle position_limit_handle(
                 position_joint_interface_.getHandle(joint_name), joint_limits, soft_limits);
diff --git a/panda_moveit_config/launch/panda_control_moveit_rviz.launch b/panda_moveit_config/launch/panda_control_moveit_rviz.launch
index 8d91dcb..7c1d0bb 100644
--- a/panda_moveit_config/launch/panda_control_moveit_rviz.launch
+++ b/panda_moveit_config/launch/panda_control_moveit_rviz.launch
@@ -6,7 +6,6 @@
   <include file="$(find franka_control)/launch/franka_control.launch" >
     <arg name="robot_ip" value="$(arg robot_ip)" />
     <arg name="load_gripper" value="$(arg load_gripper)" />
-    <arg name="publish_desired_values" value="true" />
   </include>
 
   <include if="$(arg load_gripper)" file="$(find franka_gripper)/launch/franka_gripper.launch" >
-- 
GitLab