From a9db0e775f9407c7abb6feb22a3f4a1cd9f87797 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 14 Mar 2018 14:18:17 -0300 Subject: [PATCH] Initial version split from twil_controllers. --- .gitignore | 51 +++ CMakeLists.txt | 213 +++++++++++ config/nonsmooth_backstep_controller.yaml | 1 + config/twil.yaml | 21 ++ config/twil_step.yaml | 21 ++ .../nonsmooth_backstep_controller.h | 99 +++++ launch/adaptive_nonsmooth_backstep.launch | 7 + launch/gazebo.launch | 16 + launch/gazebo8.launch | 26 ++ launch/nonsmooth_backstep.launch | 6 + msg/NonSmoothBackstepControllerStatus.msg | 16 + msg/PosePolar.msg | 3 + nonsmooth_backstep_controller_plugins.xml | 15 + package.xml | 69 ++++ scripts/pose_step.py | 32 ++ scripts/pose_step.sh | 3 + src/nonsmooth_backstep_controller.cpp | 403 +++++++++++++++++++++ 17 files changed, 1002 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 120000 config/nonsmooth_backstep_controller.yaml create mode 100644 config/twil.yaml create mode 100644 config/twil_step.yaml create mode 100644 include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h create mode 100644 launch/adaptive_nonsmooth_backstep.launch create mode 100644 launch/gazebo.launch create mode 100644 launch/gazebo8.launch create mode 100644 launch/nonsmooth_backstep.launch create mode 100644 msg/NonSmoothBackstepControllerStatus.msg create mode 100644 msg/PosePolar.msg create mode 100644 nonsmooth_backstep_controller_plugins.xml create mode 100644 package.xml create mode 100755 scripts/pose_step.py create mode 100755 scripts/pose_step.sh create mode 100644 src/nonsmooth_backstep_controller.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..35d74bb --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..195a64e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,213 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nonsmooth_backstep_controller) + +## 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 + effort_controllers + geometry_msgs + message_generation + message_runtime + nav_msgs + roscpp + tf +) +find_package(cmake_modules REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen REQUIRED) + + +## 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 run_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 run_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 + NonSmoothBackstepControllerStatus.msg + PosePolar.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 + geometry_msgs + nav_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 run_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 you 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 nonsmooth_backstep_controller + CATKIN_DEPENDS controller_interface effort_controllers geometry_msgs message_generation message_runtime nav_msgs roscpp tf + DEPENDS Eigen +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +# TODO: Check names of system library include directories (Eigen) + ${Eigen_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/nonsmooth_backstep_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}) + +## 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/nonsmooth_backstep_controller_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} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Eigen_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 +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_nonsmooth_backstep_controller.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/config/nonsmooth_backstep_controller.yaml b/config/nonsmooth_backstep_controller.yaml new file mode 120000 index 0000000..2bfa2ed --- /dev/null +++ b/config/nonsmooth_backstep_controller.yaml @@ -0,0 +1 @@ +twil.yaml \ No newline at end of file diff --git a/config/twil.yaml b/config/twil.yaml new file mode 100644 index 0000000..cd3137d --- /dev/null +++ b/config/twil.yaml @@ -0,0 +1,21 @@ +# Watch-out: The indentation here is relevant to the semantic! + +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + +nonsmooth_backstep_controller: + type: effort_controllers/NonSmoothBackstepController + joints: + - left_wheel_joint + - right_wheel_joint + F: [0.0, 0.08444758509282763, 3.770688129256381, 0.0] + G: [2.6468901285322475, 2.6468901285322475, -16.084061415321404, 16.084061415321404] + lambda: [200.0, 6.0, 6.0, 500.0, 1000.0] + gamma: [10.0, 1.0, 10.0, 50.0] + wheel_separation: 0.322 + wheel_radius: [0.075, 0.075] + odom_frame_id: "odom" + base_frame_id: "twil_origin" + priority: 99 + time_step: 0.01 diff --git a/config/twil_step.yaml b/config/twil_step.yaml new file mode 100644 index 0000000..7a11ee2 --- /dev/null +++ b/config/twil_step.yaml @@ -0,0 +1,21 @@ +# Watch-out: The indentation here is relevant to the semantic! + +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + +nonsmooth_backstep_controller: + type: effort_controllers/NonSmoothBackstepController + joints: + - left_wheel_joint + - right_wheel_joint + F: [0.0, 0.08444758509282763, 3.770688129256381, 0.0] + G: [2.6468901285322475, 2.6468901285322475, -16.084061415321404, 16.084061415321404] + lambda: [200.0, 40.0, 40.0, 500.0, 1000.0] + gamma: [10.0, 1.0, 10.0, 50.0] + wheel_separation: 0.322 + wheel_radius: [0.075, 0.075] + odom_frame_id: "odom" + base_frame_id: "twil_origin" + priority: 99 + time_step: 0.01 diff --git a/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h new file mode 100644 index 0000000..04a8c5a --- /dev/null +++ b/include/nonsmooth_backstep_controller/nonsmooth_backstep_controller.h @@ -0,0 +1,99 @@ +/****************************************************************************** + ROS nonsmooth_backstep_controller Package + Non-Smooth Backstepping Controller + Copyright (C) 2014..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef NONSMOOTH_BACKSTEP_CONTROLLER_NONSMOOTH_BACKSTEP_CONTROLLER_H +#define NONSMOOTH_BACKSTEP_CONTROLLER_NONSMOOTH_BACKSTEP_CONTROLLER_H + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace effort_controllers +{ + class NonSmoothBackstepController: public controller_interface:: + Controller + { + public: + NonSmoothBackstepController(void); + ~NonSmoothBackstepController(void); + + bool init(hardware_interface::EffortJointInterface *robot, + ros::NodeHandle &n); + void starting(const ros::Time& time); + void update(const ros::Time& time,const ros::Duration& duration); + + private: + ros::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + + boost::scoped_ptr + > status_publisher_ ; + + boost::shared_ptr > odom_publisher_; + boost::shared_ptr > tf_odom_publisher_; + + ros::Subscriber sub_command_; + ros::Subscriber sub_parameters_; + + Eigen::Matrix2d Ginv_; + Eigen::Matrix2d F_; + + std::vector wheelRadius_; + double wheelBase_; + + Eigen::Vector3d x_; + Eigen::Vector3d xRef_; + + Eigen::Vector2d eta_; + + double time_step_; + ros::Time lastSamplingTime_; + + Eigen::Vector2d phi_; + + std::vector lambda_; + std::vector gamma_; + + void commandCB(const geometry_msgs::Pose2D::ConstPtr &command); + void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); + }; +} +#endif diff --git a/launch/adaptive_nonsmooth_backstep.launch b/launch/adaptive_nonsmooth_backstep.launch new file mode 100644 index 0000000..11e443c --- /dev/null +++ b/launch/adaptive_nonsmooth_backstep.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/launch/gazebo.launch b/launch/gazebo.launch new file mode 100644 index 0000000..c6c00d3 --- /dev/null +++ b/launch/gazebo.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/launch/gazebo8.launch b/launch/gazebo8.launch new file mode 100644 index 0000000..a6869f7 --- /dev/null +++ b/launch/gazebo8.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/nonsmooth_backstep.launch b/launch/nonsmooth_backstep.launch new file mode 100644 index 0000000..bb61f47 --- /dev/null +++ b/launch/nonsmooth_backstep.launch @@ -0,0 +1,6 @@ + + + + + diff --git a/msg/NonSmoothBackstepControllerStatus.msg b/msg/NonSmoothBackstepControllerStatus.msg new file mode 100644 index 0000000..3012416 --- /dev/null +++ b/msg/NonSmoothBackstepControllerStatus.msg @@ -0,0 +1,16 @@ +Header header +geometry_msgs/Pose2D set_point +geometry_msgs/Pose2D process_value +geometry_msgs/Pose2D process_value_dot +geometry_msgs/Pose2D error +float64 time_step +float64[2] command +float64[5] lambda +float64[4] gamma +PosePolar polar_error +float64[2] backstep_set_point +float64[2] backstep_set_point_dot +float64[2] backstep_process_value +float64[2] backstep_error +float64[2] backstep_command +float64[2] linear_dynamics_command diff --git a/msg/PosePolar.msg b/msg/PosePolar.msg new file mode 100644 index 0000000..14a00fa --- /dev/null +++ b/msg/PosePolar.msg @@ -0,0 +1,3 @@ +float64 range +float64 angle +float64 orientation diff --git a/nonsmooth_backstep_controller_plugins.xml b/nonsmooth_backstep_controller_plugins.xml new file mode 100644 index 0000000..dd9fb48 --- /dev/null +++ b/nonsmooth_backstep_controller_plugins.xml @@ -0,0 +1,15 @@ + + + + + The NonSmoothBackstepController implements a non-smooth controller + based on a discontinuous transformation and backstepping. A feedback + linearization of the mobile robot dynamic model is also performed. The + inputs to the controller are the reference Cartesian pose. It expects a + EffortJointInterface type of hardware interface. + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..3fc77b8 --- /dev/null +++ b/package.xml @@ -0,0 +1,69 @@ + + + nonsmooth_backstep_controller + 3.0.0 + The nonsmooth_backstep_controller package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + Eigen + controller_interface + effort_controllers + geometry_msgs + message_generation + message_runtime + nav_msgs + roscpp + tf + Eigen + controller_interface + effort_controllers + geometry_msgs + message_generation + message_runtime + nav_msgs + roscpp + tf + + + + + + + + diff --git a/scripts/pose_step.py b/scripts/pose_step.py new file mode 100755 index 0000000..0605032 --- /dev/null +++ b/scripts/pose_step.py @@ -0,0 +1,32 @@ +#!/usr/bin/python + +import sys +import time + +import rospy +from geometry_msgs.msg import Pose2D + +def step(): + if len(sys.argv) < 4: + print 'pose_step.py x y theta' + exit() + pose=Pose2D() + pose.x=float(sys.argv[1]) + pose.y=float(sys.argv[2]) + pose.theta=float(sys.argv[3]) + + pub=rospy.Publisher('/nonsmooth_backstep_controller/command', Pose2D, queue_size=1) + + rospy.init_node('pose_step',anonymous=True) + + pub.publish(pose) + time.sleep(1) + + pub.publish(pose) + time.sleep(1) + +if __name__ == '__main__': + try: + step() + except rospy.ROSInterruptException: + pass diff --git a/scripts/pose_step.sh b/scripts/pose_step.sh new file mode 100755 index 0000000..d8179f3 --- /dev/null +++ b/scripts/pose_step.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +rostopic pub -1 /nonsmooth_backstep_controller/command geometry_msgs/Pose2D "{x: $1, y: $2, theta: $3}" diff --git a/src/nonsmooth_backstep_controller.cpp b/src/nonsmooth_backstep_controller.cpp new file mode 100644 index 0000000..47ac6d8 --- /dev/null +++ b/src/nonsmooth_backstep_controller.cpp @@ -0,0 +1,403 @@ +/****************************************************************************** + ROS nonsmooth_backstep_controller Package + Non-Smooth Backstepping Controller + Copyright (C) 2014..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + +#include + +#include + +#include + +#define sqr(x) (x*x) +#define cub(x) (x*x*x) + +namespace effort_controllers +{ + NonSmoothBackstepController::NonSmoothBackstepController(void): + wheelRadius_(2),lambda_(5),gamma_(4) + { + } + + NonSmoothBackstepController::~NonSmoothBackstepController(void) + { + sub_command_.shutdown(); + sub_parameters_.shutdown(); + } + + bool NonSmoothBackstepController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) + { + node_=n; + robot_=robot; + + XmlRpc::XmlRpcValue joint_names; + if(!node_.getParam("joints",joint_names)) + { + ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); + return false; + } + + if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); + return false; + } + + for(int i=0; i < joint_names.size();i++) + { + XmlRpc::XmlRpcValue &name_value=joint_names[i]; + if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) + { + ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); + return false; + } + + hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); + joints_.push_back(j); + } + + std::string odom_frame_id="odom"; + node_.param("odom_frame_id",odom_frame_id,odom_frame_id); + + std::string base_frame_id="base_link"; + node_.param("base_frame_id",base_frame_id,base_frame_id); + + status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); + status_publisher_->msg_.header.frame_id=odom_frame_id; + + odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"odom",100)); + odom_publisher_->msg_.header.frame_id=odom_frame_id; + odom_publisher_->msg_.child_frame_id=base_frame_id; + odom_publisher_->msg_.pose.pose.position.z=0; + odom_publisher_->msg_.pose.pose.orientation.x=0; + odom_publisher_->msg_.pose.pose.orientation.y=0; + + // Fake covariance + double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; + odom_publisher_->msg_.pose.covariance=boost::assign::list_of + (pose_cov[0]) (0) (0) (0) (0) (0) + (0) (pose_cov[1]) (0) (0) (0) (0) + (0) (0) (pose_cov[2]) (0) (0) (0) + (0) (0) (0) (pose_cov[3]) (0) (0) + (0) (0) (0) (0) (pose_cov[4]) (0) + (0) (0) (0) (0) (0) (pose_cov[5]); + + odom_publisher_->msg_.twist.twist.linear.z=0; + odom_publisher_->msg_.twist.twist.angular.x=0; + odom_publisher_->msg_.twist.twist.angular.y=0; + + //Fake covariance + double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; + odom_publisher_->msg_.twist.covariance=boost::assign::list_of + (twist_cov[0]) (0) (0) (0) (0) (0) + (0) (twist_cov[1]) (0) (0) (0) (0) + (0) (0) (twist_cov[2]) (0) (0) (0) + (0) (0) (0) (twist_cov[3]) (0) (0) + (0) (0) (0) (0) (twist_cov[4]) (0) + (0) (0) (0) (0) (0) (twist_cov[5]); + + tf_odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"/tf",100)); + tf_odom_publisher_->msg_.transforms.resize(1); + tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; + tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; + tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; + tf_odom_publisher_->msg_.transforms[0].child_frame_id=base_frame_id; + tf_odom_publisher_->msg_.transforms[0].header.frame_id=odom_frame_id; + + sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this); + + sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,this); + + if(!node_.getParam("wheel_separation",wheelBase_)) + { + ROS_ERROR("No 'wheel_separation' in controller %s.",node_.getNamespace().c_str()); + return false; + } + + wheelRadius_.resize(joints_.size()); + if(!node_.getParam("wheel_radius",wheelRadius_)) + { + ROS_ERROR("No 'wheel_radius' in controller %s.",node_.getNamespace().c_str()); + return false; + } + + gamma_[0]=10.0; + gamma_[1]=1.0; + gamma_[2]=10.0; + gamma_[3]=50.0; + node_.param("gamma",gamma_,gamma_); + + lambda_[0]=200.0; + lambda_[1]=6.0; + lambda_[2]=6.0; + lambda_[3]=500.0; + lambda_[4]=1000.0; + node_.param("lambda",lambda_,lambda_); + + std::vector FVec; + if(!node_.getParam("F",FVec)) + { + ROS_ERROR("No 'F' in controller %s.",node_.getNamespace().c_str()); + return false; + } + F_=Eigen::Map(FVec.data(),2,2).transpose(); + + std::vector GVec; + if(!node_.getParam("G",GVec)) + { + ROS_ERROR("No 'G' in controller %s.",node_.getNamespace().c_str()); + return false; + } + Ginv_=Eigen::Map(GVec.data(),2,2).transpose().inverse(); + + node_.param("time_step",time_step_,0.01); + + return true; + } + + void NonSmoothBackstepController::starting(const ros::Time& time) + { + x_.setZero(); + xRef_.setZero(); + eta_.setZero(); + lastSamplingTime_=time; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + + struct sched_param param; + if(!node_.getParam("priority",param.sched_priority)) + { + ROS_WARN("No 'priority' configured for controller %s. Using highest possible priority.",node_.getNamespace().c_str()); + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + } + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); + } + + void NonSmoothBackstepController::update(const ros::Time& time, + const ros::Duration& duration) + { + double dt=time.toSec()-lastSamplingTime_.toSec(); + + if(fabs(dt-time_step_) > time_step_/20) return; + lastSamplingTime_=time; + + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement + Eigen::Vector2d deltaPhi=-phi_; + for(unsigned int i=0;i < joints_.size();i++) + { + phi_[i]=joints_[i].getPosition(); + } + deltaPhi+=phi_; + + // Estimate pose by odometry + Eigen::Vector2d deltaU; + deltaU << (deltaPhi[0]*wheelRadius_[0]+deltaPhi[1]*wheelRadius_[1])/2.0, + (deltaPhi[1]*wheelRadius_[1]-deltaPhi[0]*wheelRadius_[0])/wheelBase_; + + double deltaS=(fabs(deltaU[1]) > DBL_EPSILON)? + deltaU[0]*sin(deltaU[1]/2)/(deltaU[1]/2):deltaU[0]; + + Eigen::Vector3d deltaX; + deltaX << deltaS*cos(x_[2]+deltaU[1]/2.0), + deltaS*sin(x_[2]+deltaU[1]/2.0), + deltaU[1]; + + x_+=deltaX; + + Eigen::Vector2d u=deltaU/dt; + + // Change of coordinates + Eigen::Matrix3d R; + R << cos(xRef_[2]), sin(xRef_[2]), 0.0, + -sin(xRef_[2]), cos(xRef_[2]), 0.0, + 0.0, 0.0, 1.0; + Eigen::Vector3d xBar=R*(x_-xRef_); + + // Discontinuous transformation + double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); + double psi=atan2(xBar[1],xBar[0]); + double alpha=xBar[2]-psi; + + // deta=(eta(k)-eta(k-1)/dt + Eigen::Vector2d deta=-eta_; + + // Backstepping + eta_[0]=-gamma_[0]*e*cos(alpha); + + if(fabs(alpha ) > DBL_EPSILON) eta_[1]=-gamma_[1]*alpha + -gamma_[0]*sin(alpha)*cos(alpha)+gamma_[0]*lambda_[2]*psi*sin(alpha)/ + lambda_[1]/alpha*cos(alpha); + else eta_[1]=gamma_[0]*lambda_[2]*psi/lambda_[1]; + deta+=eta_; + deta/=dt; + + Eigen::Vector2d eb=u-eta_; + + Eigen::Vector2d vBar; + if(fabs(e) > DBL_EPSILON) vBar[0]=-gamma_[2]*eb[0] + -lambda_[0]/lambda_[3]*e*cos(alpha) + +lambda_[1]/lambda_[3]*alpha*sin(alpha)/e + -lambda_[2]/lambda_[3]*psi*sin(alpha)/e; + else vBar[0]=-gamma_[2]*eb[0]-lambda_[0]/lambda_[3]*e*cos(alpha); + vBar[1]=-gamma_[3]*eb[1]-lambda_[1]/lambda_[4]*alpha; + + Eigen::Vector2d v=vBar+deta; + + // Linearization + Eigen::Vector2d uf(u[0]*u[1],sqr(u[1])); + Eigen::Vector2d torque=Ginv_*(v-F_*uf); + + // Apply torques + for(unsigned int i=0;i < joints_.size();i++) + { + joints_[i].setCommand(torque[i]); + } + + if(status_publisher_ && status_publisher_->trylock()) + { + status_publisher_->msg_.header.stamp=time; + + status_publisher_->msg_.set_point.x=xRef_[0]; + status_publisher_->msg_.set_point.y=xRef_[1]; + status_publisher_->msg_.set_point.theta=xRef_[2]; + + status_publisher_->msg_.process_value.x=x_[0]; + status_publisher_->msg_.process_value.y=x_[1]; + status_publisher_->msg_.process_value.theta=x_[2]; + + status_publisher_->msg_.process_value_dot.x=deltaX[0]/dt; + status_publisher_->msg_.process_value_dot.y=deltaX[1]/dt; + status_publisher_->msg_.process_value_dot.theta=deltaX[2]/dt; + + status_publisher_->msg_.error.x=xRef_[0]-x_[0]; + status_publisher_->msg_.error.y=xRef_[1]-x_[1]; + status_publisher_->msg_.error.theta=xRef_[2]-x_[2]; + + status_publisher_->msg_.time_step=dt; + + for(int i=0;i < torque.size();i++) + status_publisher_->msg_.command[i]=torque[i]; + + for(int i=0;i < lambda_.size();i++) + status_publisher_->msg_.lambda[i]=lambda_[i]; + + for(int i=0;i < gamma_.size();i++) + status_publisher_->msg_.gamma[i]=gamma_[i]; + + status_publisher_->msg_.polar_error.range=e; + status_publisher_->msg_.polar_error.angle=psi; + status_publisher_->msg_.polar_error.orientation=alpha; + + for(int i=0;i < eta_.size();i++) + status_publisher_->msg_.backstep_set_point[i]=eta_[i]; + + for(int i=0;i < deta.size();i++) + status_publisher_->msg_.backstep_set_point_dot[i]=deta[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.backstep_process_value[i]=u[i]; + + for(int i=0;i < eb.size();i++) + status_publisher_->msg_.backstep_error[i]=eb[i]; + + for(int i=0;i < vBar.size();i++) + status_publisher_->msg_.backstep_command[i]=vBar[i]; + + for(int i=0;i < v.size();i++) + status_publisher_->msg_.linear_dynamics_command[i]=v[i]; + + status_publisher_->unlockAndPublish(); + } + + if(odom_publisher_ && odom_publisher_->trylock()) + { + odom_publisher_->msg_.header.stamp=time; + + odom_publisher_->msg_.pose.pose.position.x=x_[0]; + odom_publisher_->msg_.pose.pose.position.y=x_[1]; + odom_publisher_->msg_.pose.pose.orientation.z=sin(x_[2]/2); + odom_publisher_->msg_.pose.pose.orientation.w=cos(x_[2]/2); + + odom_publisher_->msg_.twist.twist.linear.x=deltaX[0]/dt; + odom_publisher_->msg_.twist.twist.linear.y=deltaX[1]/dt; + odom_publisher_->msg_.twist.twist.angular.z=deltaX[2]/dt; + + odom_publisher_->unlockAndPublish(); + } + + if(tf_odom_publisher_ && tf_odom_publisher_->trylock()) + { + geometry_msgs::TransformStamped &odom_frame= + tf_odom_publisher_->msg_.transforms[0]; + odom_frame.header.stamp=time; + odom_frame.transform.translation.x=x_[0]; + odom_frame.transform.translation.y=x_[1]; + odom_frame.transform.rotation.z=sin(x_[2]/2); + odom_frame.transform.rotation.w=cos(x_[2]/2); + + tf_odom_publisher_->unlockAndPublish(); + } + } + + void NonSmoothBackstepController::commandCB(const geometry_msgs::Pose2D::ConstPtr &command) + { + xRef_[0]=command->x; + xRef_[1]=command->y; + xRef_[2]=command->theta; + } + + void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr ¶m) + { + // data[0]=f12, data[1]=f21, data[2]=g11, data[3]=g21 + // data[4]=Pf12, data[5]=Pkf21, data[6]=Pg11, data[7]=Pg21 + // F= [0 f12 + // f21 0] + // Ginv=[0.5/g11 0.5/g21 + // 0.5/g11 -0.5/g21] + if(param->data[4] < 1e-3) F_(0,1)=param->data[0]; + if(param->data[5] < 1e-3) F_(1,0)=param->data[1]; + if(param->data[6] < 1e-3) + { + Ginv_(0,0)=0.5/param->data[2]; + Ginv_(1,0)=0.5/param->data[2]; + } + if(param->data[7] < 1e-3) + { + Ginv_(0,1)=0.5/param->data[3]; + Ginv_(1,1)=-0.5/param->data[3]; + } + } + +} + +PLUGINLIB_EXPORT_CLASS(effort_controllers::NonSmoothBackstepController, + controller_interface::ControllerBase) -- 2.12.0