From 02cef9263ebbafdeac55d0611a46fc278203c942 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Mon, 26 Mar 2018 18:13:35 -0300 Subject: [PATCH] Initial commit. --- CMakeLists.txt | 208 ++++++++++++ config/dynamics_linearizing_controller.yaml | 1 + config/twil.yaml | 34 ++ config/twist_mrac_linearizing_controller.yaml | 1 + .../dynamics_linearizing_controller.h | 93 ++++++ .../twist_mrac_linearizing_controller.h | 97 ++++++ launch/adaptive_dynamics_linearizing.launch | 5 + launch/dynamics_linearizing.launch | 6 + launch/gazebo.launch | 17 + launch/twist_mrac.launch | 6 + linearizing_controllers_plugins.xml | 40 +++ package.xml | 67 ++++ scripts/accel_step.py | 35 +++ scripts/accel_step.sh | 3 + scripts/twist_step.py | 35 +++ scripts/twist_step.sh | 3 + src/dynamics_linearizing_controller.cpp | 317 +++++++++++++++++++ src/twist_mrac_linearizing_controller.cpp | 348 +++++++++++++++++++++ 18 files changed, 1316 insertions(+) create mode 100644 CMakeLists.txt create mode 120000 config/dynamics_linearizing_controller.yaml create mode 100644 config/twil.yaml create mode 120000 config/twist_mrac_linearizing_controller.yaml create mode 100644 include/linearizing_controllers/dynamics_linearizing_controller.h create mode 100644 include/linearizing_controllers/twist_mrac_linearizing_controller.h create mode 100644 launch/adaptive_dynamics_linearizing.launch create mode 100644 launch/dynamics_linearizing.launch create mode 100644 launch/gazebo.launch create mode 100644 launch/twist_mrac.launch create mode 100644 linearizing_controllers_plugins.xml create mode 100644 package.xml create mode 100755 scripts/accel_step.py create mode 100755 scripts/accel_step.sh create mode 100755 scripts/twist_step.py create mode 100755 scripts/twist_step.sh create mode 100644 src/dynamics_linearizing_controller.cpp create mode 100644 src/twist_mrac_linearizing_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..fc6271e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,208 @@ +cmake_minimum_required(VERSION 2.8.3) +project(linearizing_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 + effort_controllers + roscpp + geometry_msgs + tf + arc_odometry +) +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 +# ) + +## 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 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 linearizing_controllers + CATKIN_DEPENDS controller_interface effort_controllers geometry_msgs roscpp tf arc_odometry + 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/dynamics_linearizing_controller.cpp + src/twist_mrac_linearizing_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/linearizing_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} ${${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} ${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_linearizing_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/config/dynamics_linearizing_controller.yaml b/config/dynamics_linearizing_controller.yaml new file mode 120000 index 0000000..2bfa2ed --- /dev/null +++ b/config/dynamics_linearizing_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..73c0ad1 --- /dev/null +++ b/config/twil.yaml @@ -0,0 +1,34 @@ +# Watch-out: The indentation here is relevant to the semantic! + +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + +dynamics_linearizing_controller: + type: effort_controllers/DynamicsLinearizingController + 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] + 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 + +twist_mrac_linearizing_controller: + type: effort_controllers/TwistMracLinearizingController + 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] + Alpha: [10.0, 10.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/twist_mrac_linearizing_controller.yaml b/config/twist_mrac_linearizing_controller.yaml new file mode 120000 index 0000000..2bfa2ed --- /dev/null +++ b/config/twist_mrac_linearizing_controller.yaml @@ -0,0 +1 @@ +twil.yaml \ No newline at end of file diff --git a/include/linearizing_controllers/dynamics_linearizing_controller.h b/include/linearizing_controllers/dynamics_linearizing_controller.h new file mode 100644 index 0000000..b738ab9 --- /dev/null +++ b/include/linearizing_controllers/dynamics_linearizing_controller.h @@ -0,0 +1,93 @@ +/****************************************************************************** + ROS linearizing_controllers Package + Dynamics Linearizing 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 LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H +#define LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace effort_controllers +{ + class DynamicsLinearizingController: public controller_interface:: + Controller + { + public: + DynamicsLinearizingController(void); + ~DynamicsLinearizingController(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_; + + arc_odometry::DiffOdometry odom_; + + Eigen::Vector2d v_; + + double time_step_; + ros::Time lastSamplingTime_; + + Eigen::Vector2d phi_; + + void commandCB(const geometry_msgs::Accel &command); + void parametersCB(const std_msgs::Float64MultiArray &command); + }; +} +#endif diff --git a/include/linearizing_controllers/twist_mrac_linearizing_controller.h b/include/linearizing_controllers/twist_mrac_linearizing_controller.h new file mode 100644 index 0000000..1d5a92d --- /dev/null +++ b/include/linearizing_controllers/twist_mrac_linearizing_controller.h @@ -0,0 +1,97 @@ +/****************************************************************************** + ROS linearizing_controllers Package + Twist MRAC Linearizing Controller + Copyright (C) 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 LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H +#define LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace effort_controllers +{ + class TwistMracLinearizingController: public controller_interface:: + Controller + { + public: + TwistMracLinearizingController(void); + ~TwistMracLinearizingController(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_; + + arc_odometry::DiffOdometry odom_; + + Eigen::Vector2d uRef_; + + double time_step_; + ros::Time lastSamplingTime_; + + Eigen::Vector2d phi_; + + Eigen::Vector2d uModel_; + + Eigen::DiagonalMatrix Alpha_; + + void commandCB(const geometry_msgs::Twist &command); + void parametersCB(const std_msgs::Float64MultiArray &command); + }; +} +#endif diff --git a/launch/adaptive_dynamics_linearizing.launch b/launch/adaptive_dynamics_linearizing.launch new file mode 100644 index 0000000..3d601bf --- /dev/null +++ b/launch/adaptive_dynamics_linearizing.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/launch/dynamics_linearizing.launch b/launch/dynamics_linearizing.launch new file mode 100644 index 0000000..122c95c --- /dev/null +++ b/launch/dynamics_linearizing.launch @@ -0,0 +1,6 @@ + + + + + diff --git a/launch/gazebo.launch b/launch/gazebo.launch new file mode 100644 index 0000000..5aceccb --- /dev/null +++ b/launch/gazebo.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/launch/twist_mrac.launch b/launch/twist_mrac.launch new file mode 100644 index 0000000..2238629 --- /dev/null +++ b/launch/twist_mrac.launch @@ -0,0 +1,6 @@ + + + + + diff --git a/linearizing_controllers_plugins.xml b/linearizing_controllers_plugins.xml new file mode 100644 index 0000000..c7d95c6 --- /dev/null +++ b/linearizing_controllers_plugins.xml @@ -0,0 +1,40 @@ + + + + + The DynamicsLinearizingController linearizes the dynamics of + a differential-drive mobile robot. The linearized inputs are linear + and angular accelerations. It expects a EffortJointInterface type of + hardware interface. + + + + + + The TwistMracLinearizingController implements a Model Reference + (Adaptive) Linearizing Controller for the twist of a + differential-drive mobile robot. The reference inputs are the linear + and angular velocities of the robot. It expects a + EffortJointInterface type of hardware interface. + + + + + + + + The PositionMracLinearizingController implements a Model Reference + (Adaptive) Linearizing Controller for the position of a + differential-drive mobile robot. The reference input is the postion + of a reference point the robot, which can not be the center of the + wheels. It expects a EffortJointInterface type of hardware interface. + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..c742c04 --- /dev/null +++ b/package.xml @@ -0,0 +1,67 @@ + + + linearizing_controllers + 0.0.0 + The linearizing_controllers package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + Eigen + controller_interface + effort_controllers + geometry_msgs + tf + arc_odometry + linearizing_controllers_msgs + roscpp + Eigen + controller_interface + effort_controllers + geometry_msgs + tf + arc_odometry + linearizing_controllers_msgs + roscpp + + + + + + + + diff --git a/scripts/accel_step.py b/scripts/accel_step.py new file mode 100755 index 0000000..0ad1bb0 --- /dev/null +++ b/scripts/accel_step.py @@ -0,0 +1,35 @@ +#!/usr/bin/python + +import sys +import time + +import rospy +from geometry_msgs.msg import Accel + +def step(): + if len(sys.argv) < 3: + print 'accel_step.py v w' + exit() + accel=Accel() + accel.linear.x=float(sys.argv[1]) + accel.linear.y=0.0 + accel.linear.z=0.0 + accel.angular.x=0.0 + accel.angular.y=0.0 + accel.angular.z=float(sys.argv[2]) + + pub=rospy.Publisher('/dynamics_linearizing_controller/command', Accel, queue_size=1) + + rospy.init_node('accel_step',anonymous=True) + + pub.publish(accel) + time.sleep(1) + + pub.publish(accel) + time.sleep(1) + +if __name__ == '__main__': + try: + step() + except rospy.ROSInterruptException: + pass diff --git a/scripts/accel_step.sh b/scripts/accel_step.sh new file mode 100755 index 0000000..9ed33b5 --- /dev/null +++ b/scripts/accel_step.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +rostopic pub -1 /dynamics_linearizing_controller/command geometry_msgs/Accel "{linear: {x: $1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: $2}}" diff --git a/scripts/twist_step.py b/scripts/twist_step.py new file mode 100755 index 0000000..0a7d0a9 --- /dev/null +++ b/scripts/twist_step.py @@ -0,0 +1,35 @@ +#!/usr/bin/python + +import sys +import time + +import rospy +from geometry_msgs.msg import Twist + +def step(): + if len(sys.argv) < 3: + print 'twist_step.py v w' + exit() + twist=Twist() + twist.linear.x=float(sys.argv[1]) + twist.linear.y=0.0 + twist.linear.z=0.0 + twist.angular.x=0.0 + twist.angular.y=0.0 + twist.angular.z=float(sys.argv[2]) + + pub=rospy.Publisher('/twist_mrac_linearizing_controller/command', Twist, queue_size=1) + + rospy.init_node('twist_step',anonymous=True) + + pub.publish(twist) + time.sleep(1) + + pub.publish(twist) + time.sleep(1) + +if __name__ == '__main__': + try: + step() + except rospy.ROSInterruptException: + pass diff --git a/scripts/twist_step.sh b/scripts/twist_step.sh new file mode 100755 index 0000000..4ab343a --- /dev/null +++ b/scripts/twist_step.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +rostopic pub -1 /twist_mrac_linearizing_controller/command geometry_msgs/Twist "{linear: {x: $1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: $2}}" diff --git a/src/dynamics_linearizing_controller.cpp b/src/dynamics_linearizing_controller.cpp new file mode 100644 index 0000000..dd594de --- /dev/null +++ b/src/dynamics_linearizing_controller.cpp @@ -0,0 +1,317 @@ +/****************************************************************************** + ROS linearing_controllers Package + Dynamics Linearizing 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) + +namespace effort_controllers +{ + DynamicsLinearizingController::DynamicsLinearizingController(void): + odom_(0.0,std::vector(2)) + { + } + + DynamicsLinearizingController::~DynamicsLinearizingController(void) + { + sub_command_.shutdown(); + sub_parameters_.shutdown(); + } + + bool DynamicsLinearizingController::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=base_frame_id; + status_publisher_->msg_.set_point.linear.y=0.0; + status_publisher_->msg_.set_point.linear.z=0.0; + status_publisher_->msg_.set_point.angular.x=0.0; + status_publisher_->msg_.set_point.angular.y=0.0; + status_publisher_->msg_.process_value.linear.y=0.0; + status_publisher_->msg_.process_value.linear.z=0.0; + status_publisher_->msg_.process_value.angular.x=0.0; + status_publisher_->msg_.process_value.angular.y=0.0; + + 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,&DynamicsLinearizingController::commandCB,this); + + sub_parameters_=node_.subscribe("dynamic_parameters",1000,&DynamicsLinearizingController::parametersCB,this); + + double wheelBase; + if(!node_.getParam("wheel_separation",wheelBase)) + { + ROS_ERROR("No 'wheel_separation' in controller %s.",node_.getNamespace().c_str()); + return false; + } + + std::vector wheelRadius(2); + if(!node_.getParam("wheel_radius",wheelRadius)) + { + ROS_ERROR("No 'wheel_radius' in controller %s.",node_.getNamespace().c_str()); + return false; + } + + odom_.setParams(wheelBase,wheelRadius); + + 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 DynamicsLinearizingController::starting(const ros::Time& time) + { + Eigen::Vector3d x; + x.setZero(); + odom_.setPose(x); + v_.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 DynamicsLinearizingController::update(const ros::Time& time, + const ros::Duration& duration) + { + ros::Duration dt=time-lastSamplingTime_; + + if(fabs(dt.toSec()-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_; + + odom_.update(deltaPhi,dt); + + Eigen::Vector2d u; + odom_.getVelocity(u); + + // 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.linear.x=v_[0]; + status_publisher_->msg_.set_point.angular.z=v_[1]; + + status_publisher_->msg_.process_value.linear.x=u[0]; + status_publisher_->msg_.process_value.angular.z=u[1]; + + status_publisher_->msg_.time_step=dt.toSec(); + + for(int i=0;i < torque.size();i++) + status_publisher_->msg_.command[i]=torque[i]; + + status_publisher_->unlockAndPublish(); + } + + Eigen::Vector3d x; + odom_.getPose(x); + + 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=u[0]*cos(x[2]); + odom_publisher_->msg_.twist.twist.linear.y=u[0]*sin(x[2]); + odom_publisher_->msg_.twist.twist.angular.z=u[1]; + + 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 DynamicsLinearizingController::commandCB(const geometry_msgs::Accel &command) + { + v_[0]=command.linear.x; + v_[1]=command.angular.z; + } + + void DynamicsLinearizingController::parametersCB(const std_msgs::Float64MultiArray ¶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::DynamicsLinearizingController, + controller_interface::ControllerBase) diff --git a/src/twist_mrac_linearizing_controller.cpp b/src/twist_mrac_linearizing_controller.cpp new file mode 100644 index 0000000..bbc1a63 --- /dev/null +++ b/src/twist_mrac_linearizing_controller.cpp @@ -0,0 +1,348 @@ +/****************************************************************************** + ROS linearing_controllers Package + Twist MRAC Linearizing Controller + Copyright (C) 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) + +namespace effort_controllers +{ + TwistMracLinearizingController::TwistMracLinearizingController(void): + odom_(0.0,std::vector(2)) + { + } + + TwistMracLinearizingController::~TwistMracLinearizingController(void) + { + sub_command_.shutdown(); + sub_parameters_.shutdown(); + } + + bool TwistMracLinearizingController::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=base_frame_id; + status_publisher_->msg_.set_point.linear.y=0.0; + status_publisher_->msg_.set_point.linear.z=0.0; + status_publisher_->msg_.set_point.angular.x=0.0; + status_publisher_->msg_.set_point.angular.y=0.0; + status_publisher_->msg_.process_value.linear.y=0.0; + status_publisher_->msg_.process_value.linear.z=0.0; + status_publisher_->msg_.process_value.angular.x=0.0; + status_publisher_->msg_.process_value.angular.y=0.0; + status_publisher_->msg_.error.linear.y=0.0; + status_publisher_->msg_.error.linear.z=0.0; + status_publisher_->msg_.error.angular.x=0.0; + status_publisher_->msg_.error.angular.y=0.0; + + 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,&TwistMracLinearizingController::commandCB,this); + + sub_parameters_=node_.subscribe("dynamic_parameters",1000,&TwistMracLinearizingController::parametersCB,this); + + double wheelBase; + if(!node_.getParam("wheel_separation",wheelBase)) + { + ROS_ERROR("No 'wheel_separation' in controller %s.",node_.getNamespace().c_str()); + return false; + } + + std::vector wheelRadius(2); + if(!node_.getParam("wheel_radius",wheelRadius)) + { + ROS_ERROR("No 'wheel_radius' in controller %s.",node_.getNamespace().c_str()); + return false; + } + + odom_.setParams(wheelBase,wheelRadius); + + 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(); + + std::vector AlphaVec; + if(!node_.getParam("Alpha",AlphaVec)) + { + ROS_ERROR("No 'Alpha' in controller %s.",node_.getNamespace().c_str()); + return false; + } + Alpha_.diagonal()=Eigen::Map(AlphaVec.data(),2); + + node_.param("time_step",time_step_,0.01); + + return true; + } + + void TwistMracLinearizingController::starting(const ros::Time& time) + { + Eigen::Vector3d x; + x.setZero(); + + odom_.setPose(x); + uRef_.setZero(); + uModel_.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 TwistMracLinearizingController::update(const ros::Time& time, + const ros::Duration& duration) + { + ros::Duration dt=time-lastSamplingTime_; + + if(fabs(dt.toSec()-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_; + + odom_.update(deltaPhi,dt); + + Eigen::Vector2d u; + odom_.getVelocity(u); + + // Compute reference model + Eigen::Vector2d deltaUModel=Alpha_*(uRef_-uModel_); + + Eigen::Vector2d v=Alpha_*(uModel_-u)+deltaUModel; + + //update reference model + uModel_+=deltaUModel*dt.toSec(); + + // 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.linear.x=uRef_[0]; + status_publisher_->msg_.set_point.angular.z=uRef_[1]; + + status_publisher_->msg_.process_value.linear.x=u[0]; + status_publisher_->msg_.process_value.angular.z=u[1]; + + status_publisher_->msg_.error.linear.x=uRef_[0]-u[0]; + status_publisher_->msg_.error.angular.z=uRef_[1]-u[1]; + + status_publisher_->msg_.time_step=dt.toSec(); + + for(int i=0;i < torque.size();i++) + { + status_publisher_->msg_.command[i]=torque[i]; + status_publisher_->msg_.alpha[i]=Alpha_.diagonal()(i); + status_publisher_->msg_.reference_model[i]=uModel_[i]; + status_publisher_->msg_.reference_model_dot[i]=deltaUModel[i]; + status_publisher_->msg_.reference_model_error[i]=uRef_[i]-uModel_[i]; + status_publisher_->msg_.linear_dynamics_command[i]=v[i]; + } + status_publisher_->unlockAndPublish(); + } + + Eigen::Vector3d x; + odom_.getPose(x); + + 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=u[0]*cos(x[2]); + odom_publisher_->msg_.twist.twist.linear.y=u[0]*sin(x[2]); + odom_publisher_->msg_.twist.twist.angular.z=u[1]; + + 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 TwistMracLinearizingController::commandCB(const geometry_msgs::Twist &command) + { + uRef_[0]=command.linear.x; + uRef_[1]=command.angular.z; + } + + void TwistMracLinearizingController::parametersCB(const std_msgs::Float64MultiArray ¶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::TwistMracLinearizingController, + controller_interface::ControllerBase) -- 2.12.0