diff --git a/ur_controllers/.clang-format b/ur_controllers/.clang-format new file mode 100644 index 0000000..eca6051 --- /dev/null +++ b/ur_controllers/.clang-format @@ -0,0 +1,66 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortLoopsOnASingleLine: false +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/ur_controllers/.clang-tidy b/ur_controllers/.clang-tidy new file mode 100644 index 0000000..d935e37 --- /dev/null +++ b/ur_controllers/.clang-tidy @@ -0,0 +1,10 @@ +Checks: '-*,readability-identifier-naming' +CheckOptions: + - { key: readability-identifier-naming.NamespaceCase, value: lower_case } + - { key: readability-identifier-naming.ClassCase, value: CamelCase } + - { key: readability-identifier-naming.PrivateMemberSuffix, value: _ } + - { key: readability-identifier-naming.StructCase, value: CamelCase } + - { key: readability-identifier-naming.FunctionCase, value: camelBack } + - { key: readability-identifier-naming.VariableCase, value: lower_case } + - { key: readability-identifier-naming.GlobalVariablePrefix, value: g_ } + - { key: readability-identifier-naming.GlobalConstantCase, value: UPPER_CASE } diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt new file mode 100644 index 0000000..706438b --- /dev/null +++ b/ur_controllers/CMakeLists.txt @@ -0,0 +1,210 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ur_controllers) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + controller_interface + hardware_interface + joint_trajectory_controller + pluginlib + realtime_tools + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES ur_controllers + CATKIN_DEPENDS + controller_interface + hardware_interface + joint_trajectory_controller + pluginlib + realtime_tools + std_msgs + # DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/scaled_joint_trajectory_controller.cpp + src/speed_scaling_state_controller.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/ur_controllers_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN ".svn" + EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES controller_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ur_controllers.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml new file mode 100644 index 0000000..8539639 --- /dev/null +++ b/ur_controllers/controller_plugins.xml @@ -0,0 +1,12 @@ + + + + This controller publishes the readings of all available speed scaling factors. + + + + + Scaled joint trajectory controller + + + diff --git a/ur_controllers/include/ur_controllers/hardware_interface_adapter.h b/ur_controllers/include/ur_controllers/hardware_interface_adapter.h new file mode 100644 index 0000000..286d81b --- /dev/null +++ b/ur_controllers/include/ur_controllers/hardware_interface_adapter.h @@ -0,0 +1,84 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-18 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED +#define UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED + + +#include +#include "ur_controllers/scaled_joint_command_interface.h" + +/** + * \brief Adapter for a position-controlled hardware interface. Forwards desired positions as commands. + * + * The following is an example configuration of a controller that uses this adapter. + * \code + * head_controller: + * type: "position_controllers/ScaledJointTrajectoryController" + * joints: + * - head_1_joint + * - head_2_joint + * + * constraints: + * goal_time: 0.6 + * stopped_velocity_tolerance: 0.02 + * head_1_joint: {trajectory: 0.05, goal: 0.02} + * head_2_joint: {trajectory: 0.05, goal: 0.02} + * stop_trajectory_duration: 0.5 + * state_publish_rate: 25 + * \endcode + */ +template +class HardwareInterfaceAdapter +{ +public: + HardwareInterfaceAdapter() : joint_handles_ptr_(0) {} + + bool init(std::vector& joint_handles, ros::NodeHandle& /*controller_nh*/) + { + // Store pointer to joint handles + joint_handles_ptr_ = &joint_handles; + + + return true; + } + + void starting(const ros::Time& /*time*/) + { + if (!joint_handles_ptr_) {return;} + + // Semantic zero for commands + for (auto& jh : *joint_handles_ptr_) + { + jh.setCommand(jh.getPosition()); + } + } + + void stopping(const ros::Time& /*time*/) {} + + void updateCommand(const ros::Time& /*time*/, + const ros::Duration& /*period*/, + const State& desired_state, + const State& /*state_error*/) + { + // Forward desired position to command + const unsigned int n_joints = joint_handles_ptr_->size(); + for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);} + } + +private: + std::vector* joint_handles_ptr_; +}; + +#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED diff --git a/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h new file mode 100644 index 0000000..7a828f1 --- /dev/null +++ b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h @@ -0,0 +1,139 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright (C) 2012, hiDOF INC. +// Copyright (C) 2019, FZI Forschungszentrum Informatik (Scaling extension) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of hiDOF, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +/// \author Wim Meeussen +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-18 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED +#define UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED + +#include +#include + +namespace ur_controllers +{ +class ScaledJointHandle : public hardware_interface::JointStateHandle +{ +public: + ScaledJointHandle() : cmd_(0), scaling_factor_(0) + { + } + + /** + * \param js This joint's state handle + * \param cmd A pointer to the storage for this joint's output command + */ + ScaledJointHandle(const hardware_interface::JointStateHandle& js, double* cmd, double* scaling_factor) + : hardware_interface::JointStateHandle(js), cmd_(cmd), scaling_factor_(scaling_factor) + { + if (cmd_ == nullptr) + { + throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + + "'. Command data pointer is null."); + } + if (scaling_factor_ == nullptr) + { + throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + + "'. Scaling factor pointer is null."); + } + } + + virtual ~ScaledJointHandle() = default; + + void setCommand(double command) + { + assert(cmd_); + *cmd_ = command; + } + double getCommand() const + { + assert(cmd_); + return *cmd_; + } + void setScalingFactor(double scaling_factor) + { + assert(scaling_factor_); + *scaling_factor_ = scaling_factor; + } + double getScalingFactor() const + { + assert(scaling_factor_); + return *scaling_factor_; + } + +private: + double* cmd_; + double* scaling_factor_; +}; + +/** \brief Hardware interface to support commanding an array of joints. + * + * This \ref HardwareInterface supports commanding the output of an array of + * named joints. Note that these commands can have any semantic meaning as long + * as they each can be represented by a single double, they are not necessarily + * effort commands. To specify a meaning to this command, see the derived + * classes like \ref EffortJointInterface etc. + * + * These interfaces provide an additional scaling factor implemented, e.g. by a robot's speed + * slider. + * + * + * \note Getting a joint handle through the getHandle() method \e will claim that resource. + * + */ +class ScaledJointCommandInterface + : public hardware_interface::HardwareResourceManager +{ +}; + +/// \ref ScaledJointCommandInterface for commanding effort-based joints. +class ScaledEffortJointInterface : public ScaledJointCommandInterface +{ +}; + +/// \ref ScaledJointCommandInterface for commanding velocity-based joints. +class ScaledVelocityJointInterface : public ScaledJointCommandInterface +{ +}; + +/// \ref ScaledJointCommandInterface for commanding position-based joints. +class ScaledPositionJointInterface : public ScaledJointCommandInterface +{ +}; +} // namespace ur_controllers +#endif // ifndef UR_CONTROLLERS_SCALED_JOINT_COMMAND_INTERFACE_H_INCLUDED diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.h b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.h new file mode 100644 index 0000000..9413ece --- /dev/null +++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.h @@ -0,0 +1,193 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-18 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED +#define UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED + +#include "ur_controllers/hardware_interface_adapter.h" +#include + +namespace ur_controllers +{ +template +class ScaledJointTrajectoryController + : public joint_trajectory_controller::JointTrajectoryController +{ +public: + ScaledJointTrajectoryController() = default; + virtual ~ScaledJointTrajectoryController() = default; + + void update(const ros::Time& time, const ros::Duration& period) + { + this->scaling_factor_ = this->joints_[0].getScalingFactor(); + // Get currently followed trajectory + typename Base::TrajectoryPtr curr_traj_ptr; + this->curr_trajectory_box_.get(curr_traj_ptr); + typename Base::Trajectory& curr_traj = *curr_traj_ptr; + + // Update time data + typename Base::TimeData time_data; + time_data.time = time; // Cache current time + time_data.period = ros::Duration(this->scaling_factor_ * period.toSec()); // Cache current control period + time_data.uptime = this->time_data_.readFromRT()->uptime + time_data.period; // Update controller uptime + ros::Time traj_time = this->time_data_.readFromRT()->uptime + period; + this->time_data_.writeFromNonRT(time_data); // TODO: Grrr, we need a lock-free data structure here! + + // NOTE: It is very important to execute the two above code blocks in the specified sequence: first get current + // trajectory, then update time data. Hopefully the following paragraph sheds a bit of light on the rationale. + // The non-rt thread responsible for processing new commands enqueues trajectories that can start at the _next_ + // control cycle (eg. zero start time) or later (eg. when we explicitly request a start time in the future). + // If we reverse the order of the two blocks above, and update the time data first; it's possible that by the time + // we fetch the currently followed trajectory, it has been updated by the non-rt thread with something that starts + // in the next control cycle, leaving the current cycle without a valid trajectory. + + // Update current state and state error + for (unsigned int i = 0; i < this->joints_.size(); ++i) + { + this->current_state_.position[i] = this->joints_[i].getPosition(); + this->current_state_.velocity[i] = this->joints_[i].getVelocity(); + // There's no acceleration data available in a joint handle + + typename Base::TrajectoryPerJoint::const_iterator segment_it = + sample(curr_traj[i], traj_time.toSec(), this->desired_joint_state_); + if (curr_traj[i].end() == segment_it) + { + // Non-realtime safe, but should never happen under normal operation + ROS_ERROR_NAMED(this->name_, "Unexpected error: No trajectory defined at current time. Please contact the " + "package " + "maintainer."); + return; + } + this->desired_state_.position[i] = this->desired_joint_state_.position[0]; + this->desired_state_.velocity[i] = this->desired_joint_state_.velocity[0]; + this->desired_state_.acceleration[i] = this->desired_joint_state_.acceleration[0]; + ; + + this->state_joint_error_.position[0] = + angles::shortest_angular_distance(this->current_state_.position[i], this->desired_joint_state_.position[0]); + this->state_joint_error_.velocity[0] = this->desired_joint_state_.velocity[0] - this->current_state_.velocity[i]; + this->state_joint_error_.acceleration[0] = 0.0; + + this->state_error_.position[i] = + angles::shortest_angular_distance(this->current_state_.position[i], this->desired_joint_state_.position[0]); + this->state_error_.velocity[i] = this->desired_joint_state_.velocity[0] - this->current_state_.velocity[i]; + this->state_error_.acceleration[i] = 0.0; + + // Check tolerances + const typename Base::RealtimeGoalHandlePtr rt_segment_goal = segment_it->getGoalHandle(); + if (rt_segment_goal && rt_segment_goal == this->rt_active_goal_) + { + // Check tolerances + if (time_data.uptime.toSec() < segment_it->endTime()) + { + // Currently executing a segment: check path tolerances + const joint_trajectory_controller::SegmentTolerancesPerJoint& joint_tolerances = + segment_it->getTolerances(); + if (!checkStateTolerancePerJoint(this->state_joint_error_, joint_tolerances.state_tolerance)) + { + if (this->verbose_) + { + ROS_ERROR_STREAM_NAMED(this->name_, "Path tolerances failed for joint: " << this->joint_names_[i]); + checkStateTolerancePerJoint(this->state_joint_error_, joint_tolerances.state_tolerance, true); + } + rt_segment_goal->preallocated_result_->error_code = + control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + this->rt_active_goal_.reset(); + this->successful_joint_traj_.reset(); + } + } + else if (segment_it == --curr_traj[i].end()) + { + if (this->verbose_) + ROS_DEBUG_STREAM_THROTTLE_NAMED(1, this->name_, + "Finished executing last segment, checking goal tolerances"); + + // Controller uptime + const ros::Time uptime = this->time_data_.readFromRT()->uptime; + + // Checks that we have ended inside the goal tolerances + const joint_trajectory_controller::SegmentTolerancesPerJoint& tolerances = + segment_it->getTolerances(); + const bool inside_goal_tolerances = + checkStateTolerancePerJoint(this->state_joint_error_, tolerances.goal_state_tolerance); + + if (inside_goal_tolerances) + { + this->successful_joint_traj_[i] = 1; + } + else if (uptime.toSec() < segment_it->endTime() + tolerances.goal_time_tolerance) + { + // Still have some time left to meet the goal state tolerances + } + else + { + if (this->verbose_) + { + ROS_ERROR_STREAM_NAMED(this->name_, "Goal tolerances failed for joint: " << this->joint_names_[i]); + // Check the tolerances one more time to output the errors that occurs + checkStateTolerancePerJoint(this->state_joint_error_, tolerances.goal_state_tolerance, true); + } + + rt_segment_goal->preallocated_result_->error_code = + control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; + rt_segment_goal->setAborted(rt_segment_goal->preallocated_result_); + this->rt_active_goal_.reset(); + this->successful_joint_traj_.reset(); + } + } + } + } + + // If there is an active goal and all segments finished successfully then set goal as succeeded + typename Base::RealtimeGoalHandlePtr current_active_goal(this->rt_active_goal_); + if (current_active_goal && this->successful_joint_traj_.count() == this->joints_.size()) + { + current_active_goal->preallocated_result_->error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + current_active_goal->setSucceeded(current_active_goal->preallocated_result_); + current_active_goal.reset(); // do not publish feedback + this->rt_active_goal_.reset(); + this->successful_joint_traj_.reset(); + } + + // Hardware interface adapter: Generate and send commands + this->hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period, this->desired_state_, this->state_error_); + + // Set action feedback + if (current_active_goal) + { + current_active_goal->preallocated_feedback_->header.stamp = this->time_data_.readFromRT()->time; + current_active_goal->preallocated_feedback_->desired.positions = this->desired_state_.position; + current_active_goal->preallocated_feedback_->desired.velocities = this->desired_state_.velocity; + current_active_goal->preallocated_feedback_->desired.accelerations = this->desired_state_.acceleration; + current_active_goal->preallocated_feedback_->actual.positions = this->current_state_.position; + current_active_goal->preallocated_feedback_->actual.velocities = this->current_state_.velocity; + current_active_goal->preallocated_feedback_->error.positions = this->state_error_.position; + current_active_goal->preallocated_feedback_->error.velocities = this->state_error_.velocity; + current_active_goal->setFeedback(current_active_goal->preallocated_feedback_); + } + + // Publish state + this->publishState(time_data.uptime); + } + +protected: + using Base = joint_trajectory_controller::JointTrajectoryController; + double scaling_factor_; + +private: + /* data */ +}; +} // namespace ur_controllers + +#endif // ifndef UR_CONTROLLERS_SCALED_TRAJECTORY_CONTROLLER_H_INCLUDED diff --git a/ur_controllers/include/ur_controllers/speed_scaling_interface.h b/ur_controllers/include/ur_controllers/speed_scaling_interface.h new file mode 100644 index 0000000..e22eac8 --- /dev/null +++ b/ur_controllers/include/ur_controllers/speed_scaling_interface.h @@ -0,0 +1,50 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-17 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED +#define UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED + +#include + +namespace ur_controllers +{ +class SpeedScalingHandle +{ +public: + SpeedScalingHandle() : name_(""), scaling_factor_(0){}; + SpeedScalingHandle(const std::string& name, const double* scaling_factor) + : name_(name), scaling_factor_(scaling_factor){}; + virtual ~SpeedScalingHandle() = default; + + std::string getName() const + { + return name_; + } + + const double* getScalingFactor() const + { + return scaling_factor_; + } + +private: + std::string name_; + const double* scaling_factor_; +}; +/** \brief Hardware interface to support reading the speed scaling factor. */ +class SpeedScalingInterface : public hardware_interface::HardwareResourceManager +{ +}; +} // namespace ur_controllers + +#endif // ifndef UR_CONTROLLERS_SPEED_SCALING_INTERFACE_H_INCLUDED diff --git a/ur_controllers/include/ur_controllers/speed_scaling_state_controller.h b/ur_controllers/include/ur_controllers/speed_scaling_state_controller.h new file mode 100644 index 0000000..cd7ffd1 --- /dev/null +++ b/ur_controllers/include/ur_controllers/speed_scaling_state_controller.h @@ -0,0 +1,46 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-17 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED +#define UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED + +#include +#include +#include + +#include "ur_controllers/speed_scaling_interface.h" + +namespace ur_controllers +{ +class SpeedScalingStateController : public controller_interface::Controller +{ +public: + SpeedScalingStateController() = default; + virtual ~SpeedScalingStateController() override = default; + + virtual bool init(SpeedScalingInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + virtual void starting(const ros::Time& time) override; + virtual void update(const ros::Time& time, const ros::Duration& /*period*/) override; + virtual void stopping(const ros::Time& /*time*/) override; + +private: + std::vector sensors_; + //TODO: We should use a better datatype later on + typedef std::shared_ptr > RtPublisherPtr; + std::vector realtime_pubs_; + std::vector last_publish_times_; + double publish_rate_; +}; +} // namespace ur_controllers +#endif // ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml new file mode 100644 index 0000000..2d34bfd --- /dev/null +++ b/ur_controllers/package.xml @@ -0,0 +1,66 @@ + + + ur_controllers + 0.0.0 + The ur_controllers package + + + + + Felix Mauch + + + + + + FZI + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + controller_interface + hardware_interface + joint_trajectory_controller + pluginlib + realtime_tools + std_msgs + + + + + + + + diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp new file mode 100644 index 0000000..523ae19 --- /dev/null +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -0,0 +1,29 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-18 + * + */ +//---------------------------------------------------------------------- + +#include "ur_controllers/scaled_joint_trajectory_controller.h" +#include "ur_controllers/scaled_joint_command_interface.h" + +#include +#include + + +namespace position_controllers +{ +typedef ur_controllers::ScaledJointTrajectoryController, + ur_controllers::ScaledPositionJointInterface> + ScaledJointTrajectoryController; +} + +PLUGINLIB_EXPORT_CLASS(position_controllers::ScaledJointTrajectoryController, controller_interface::ControllerBase) diff --git a/ur_controllers/src/speed_scaling_state_controller.cpp b/ur_controllers/src/speed_scaling_state_controller.cpp new file mode 100644 index 0000000..c3a555c --- /dev/null +++ b/ur_controllers/src/speed_scaling_state_controller.cpp @@ -0,0 +1,89 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-17 + * + */ +//---------------------------------------------------------------------- + +#include "ur_controllers/speed_scaling_state_controller.h" + +#include + +namespace ur_controllers +{ +bool SpeedScalingStateController::init(SpeedScalingInterface* hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + // get all joint states from the hardware interface + const std::vector& sensor_names = hw->getNames(); + for (unsigned i = 0; i < sensor_names.size(); i++) + ROS_DEBUG("Got sensor %s", sensor_names[i].c_str()); + + // get publishing period + if (!controller_nh.getParam("publish_rate", publish_rate_)) + { + ROS_ERROR("Parameter 'publish_rate' not set"); + return false; + } + + for (unsigned i = 0; i < sensor_names.size(); i++) + { + // sensor handle + sensors_.push_back(hw->getHandle(sensor_names[i])); + + // realtime publisher + RtPublisherPtr rt_pub(new realtime_tools::RealtimePublisher(root_nh, sensor_names[i], 4)); + realtime_pubs_.push_back(rt_pub); + } + + // Last published times + last_publish_times_.resize(sensor_names.size()); + return true; +} + +void SpeedScalingStateController ::starting(const ros::Time& time) +{ + // initialize time + for (unsigned i = 0; i < last_publish_times_.size(); i++) + { + last_publish_times_[i] = time; + } +} + +void SpeedScalingStateController ::update(const ros::Time& time, const ros::Duration& /*period*/) +{ + // limit rate of publishing + for (unsigned i = 0; i < realtime_pubs_.size(); i++) + { + if (publish_rate_ > 0.0 && last_publish_times_[i] + ros::Duration(1.0 / publish_rate_) < time) + { + // try to publish + if (realtime_pubs_[i]->trylock()) + { + // we're actually publishing, so increment time + last_publish_times_[i] = last_publish_times_[i] + ros::Duration(1.0 / publish_rate_); + + // populate message + // realtime_pubs_[i]->msg_.header.stamp = time; + realtime_pubs_[i]->msg_.data = *sensors_[i].getScalingFactor(); + + realtime_pubs_[i]->unlockAndPublish(); + } + } + } +} + +void SpeedScalingStateController ::stopping(const ros::Time& /*time*/) +{ +} +} // namespace ur_controllers + +PLUGINLIB_EXPORT_CLASS(ur_controllers::SpeedScalingStateController, controller_interface::ControllerBase) + diff --git a/ur_rtde_driver/config/ur10_controllers.yaml b/ur_rtde_driver/config/ur10_controllers.yaml index 3166578..ec08f84 100644 --- a/ur_rtde_driver/config/ur10_controllers.yaml +++ b/ur_rtde_driver/config/ur10_controllers.yaml @@ -30,7 +30,8 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller pos_based_pos_traj_controller: - type: position_controllers/JointTrajectoryController + #type: position_controllers/JointTrajectoryController + type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint - shoulder_lift_joint diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index 48abb6f..af2a173 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -34,6 +34,7 @@ #include #include +#include #include "ur_rtde_driver/ur/ur_driver.h" @@ -58,6 +59,7 @@ protected: std::unique_ptr ur_driver_; hardware_interface::JointStateInterface js_interface_; + ur_controllers::ScaledPositionJointInterface spj_interface_; hardware_interface::PositionJointInterface pj_interface_; ur_controllers::SpeedScalingInterface speedsc_interface_; // hardware_interface::VelocityJointInterface vj_interface_; @@ -67,7 +69,9 @@ protected: vector6d_t joint_positions_; vector6d_t joint_velocities_; vector6d_t joint_efforts_; - double speed_scaling_value_; + double speed_scaling_; + double target_speed_fraction_; + double speed_scaling_combined_; std::vector joint_names_; bool position_controller_running_; diff --git a/ur_rtde_driver/launch/ur10_ros_control.launch b/ur_rtde_driver/launch/ur10_ros_control.launch index 445d971..5a67b19 100644 --- a/ur_rtde_driver/launch/ur10_ros_control.launch +++ b/ur_rtde_driver/launch/ur10_ros_control.launch @@ -16,7 +16,7 @@ - + diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index e2f94e4..557d59c 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -60,22 +60,26 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h // Create ros_control interfaces for (std::size_t i = 0; i < joint_positions_.size(); ++i) { + ROS_INFO_STREAM("Registing handles for joint " << joint_names_[i]); // Create joint state interface for all joints js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i], &joint_velocities_[i], &joint_efforts_[i])); // Create joint position control interface - pj_interface_.registerHandle( - hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); + // pj_interface_.registerHandle( + // hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); + spj_interface_.registerHandle(ur_controllers::ScaledJointHandle( + js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_)); } - speedsc_interface_.registerHandle( - ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_)); + // speedsc_interface_.registerHandle( + // ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_)); // Register interfaces registerInterface(&js_interface_); - registerInterface(&pj_interface_); - registerInterface(&speedsc_interface_); + registerInterface(&spj_interface_); + // registerInterface(&pj_interface_); + // registerInterface(&speedsc_interface_); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface"); @@ -97,11 +101,18 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period // This throwing should never happen unless misconfigured throw std::runtime_error("Did not find joint velocity in data sent from robot. This should not happen!"); } - if (!data_pkg->getData("target_speed_fraction", speed_scaling_value_)) + if (!data_pkg->getData("target_speed_fraction", target_speed_fraction_)) + { + // This throwing should never happen unless misconfigured + throw std::runtime_error("Did not find target_speed_fraction_ in data sent from robot. This should not happen!"); + } + if (!data_pkg->getData("speed_scaling", speed_scaling_)) { // This throwing should never happen unless misconfigured throw std::runtime_error("Did not find speed_scaling in data sent from robot. This should not happen!"); } + + speed_scaling_combined_ = speed_scaling_ * target_speed_fraction_; } else { @@ -113,6 +124,7 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio { if (position_controller_running_) { + // ROS_INFO_STREAM("Writing command: " << joint_position_command_); ur_driver_->writeJointCommand(joint_position_command_); } } @@ -132,7 +144,7 @@ void HardwareInterface ::doSwitch(const std::list