diff --git a/CHANGELOG.md b/CHANGELOG.md index 259ed8d5e7933fc806da2a682ad7fe362a9ed4a0..d1d879a8706e76f77119a42025a5cfe61cc43167 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,8 +6,8 @@ * Added epsilon to grasp action * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total` to the robot state - * Updated and improved examples. - * Fix includes for Eigen3 in `franka_example_controllers` + * Updated and improved examples + * Fixed includes for Eigen3 in `franka_example_controllers` * Fixed gripper state publishing in `franka_gripper_node` ## 0.1.2 - 2017-10-10 diff --git a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h index dec1ec18b63a24ce6bb9767551e91bb01c59c38f..053d86ae59eb6bfc9af5762064326ac0081100e9 100644 --- a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h @@ -49,7 +49,7 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro double filter_gain_{0.001}; Eigen::Matrix<double, 7, 1> tau_ext_initial_; Eigen::Matrix<double, 7, 1> tau_error_; - const double delta_tau_max_{1.0}; + static constexpr double kDeltaTauMax{1.0}; // Dynamic reconfigure std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>> diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h index 778cc9b8207ea128ff0a9aefc70906021bb55df8..e9734c4690b8c9f9b964d21e3d1139f0a88420e0 100644 --- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h +++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h @@ -40,7 +40,7 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_; std::vector<hardware_interface::JointHandle> joint_handles_; - const double delta_tau_max_{1.0}; + static constexpr double kDeltaTauMax{1.0}; double radius_{0.1}; double acceleration_time_{2.0}; double vel_max_{0.05}; @@ -54,8 +54,8 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf std::array<double, 16> initial_pose_; franka_hw::TriggerRate rate_trigger_{1.0}; + std::array<double, 7> last_tau_d_{}; realtime_tools::RealtimePublisher<JointTorqueComparison> torques_publisher_; - std::array<double, 7> last_tau_d_ = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; }; } // namespace franka_example_controllers diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp index 3bc8f80f2c9c169f54537930eb55a49aab5a2f39..7ce5b69d9f14919ec9a8110271266b2cd8b644e0 100644 --- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp @@ -54,8 +54,7 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) { ROS_ERROR_STREAM( "CartesianVelocityExampleController: Robot is not in the expected starting position " - "for " - "running this example. Run `roslaunch panda_moveit_config move_to_start.launch " + "for running this example. Run `roslaunch panda_moveit_config move_to_start.launch " "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first."); return false; } diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp index 15ae1cbe0d8ce9e0554889ce91ff86c1ddddb2c6..d249ce05cab61b932db6a19e87e54414fad72c8b 100644 --- a/franka_example_controllers/src/force_example_controller.cpp +++ b/franka_example_controllers/src/force_example_controller.cpp @@ -146,7 +146,7 @@ Eigen::Matrix<double, 7, 1> ForceExampleController::saturateTorqueRate( // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more. double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]); tau_d_saturated[i] = - (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); + (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); } return tau_d_saturated; } diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp index d8c9fe75a320a8ab36c722e0951aa1bfab2bf755..f00a447f6454aeca8c392254c874d0071431a65d 100644 --- a/franka_example_controllers/src/joint_impedance_example_controller.cpp +++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp @@ -15,8 +15,6 @@ namespace franka_example_controllers { bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) { std::string arm_id; - std::vector<std::string> joint_names; - if (!node_handle.getParam("arm_id", arm_id)) { ROS_ERROR("JointImpedanceExampleController: Could not read parameter arm_id"); return false; @@ -40,6 +38,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw << acceleration_time_); } + std::vector<std::string> joint_names; if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) { ROS_ERROR( "JointImpedanceExampleController: Invalid or no joint_names parameters provided, aborting " @@ -212,7 +211,7 @@ std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate( // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more. double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]); tau_d_saturated[i] = - (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); + (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); } return tau_d_saturated; }