--- /dev/null
+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)
--- /dev/null
+twil.yaml
\ No newline at end of file
--- /dev/null
+# 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
--- /dev/null
+twil.yaml
\ No newline at end of file
--- /dev/null
+/******************************************************************************
+ ROS linearizing_controllers Package
+ Dynamics Linearizing Controller
+ Copyright (C) 2014..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H
+#define LINEARIZING_CONTROLLERS_DYNAMICS_LINEARIZING_CONTROLLER_H
+
+#include <cstddef>
+#include <vector>
+#include <string>
+
+#include <Eigen/Dense>
+#include <boost/scoped_ptr.hpp>
+
+#include <ros/node_handle.h>
+
+#include <std_msgs/Float64MultiArray.h>
+#include <geometry_msgs/Accel.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/tfMessage.h>
+
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <arc_odometry/diff_odometry.h>
+#include <linearizing_controllers_msgs/DynamicsLinearizingControllerStatus.h>
+
+namespace effort_controllers
+{
+ class DynamicsLinearizingController: public controller_interface::
+ Controller<hardware_interface::EffortJointInterface>
+ {
+ 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<hardware_interface::JointHandle> joints_;
+
+ boost::scoped_ptr<realtime_tools::RealtimePublisher
+ <linearizing_controllers_msgs::DynamicsLinearizingControllerStatus>
+ > status_publisher_ ;
+
+ boost::shared_ptr<realtime_tools::RealtimePublisher
+ <nav_msgs::Odometry> > odom_publisher_;
+ boost::shared_ptr<realtime_tools::RealtimePublisher
+ <tf::tfMessage> > 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
--- /dev/null
+/******************************************************************************
+ ROS linearizing_controllers Package
+ Twist MRAC Linearizing Controller
+ Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H
+#define LINEARIZING_CONTROLLERS_TWIST_MRAC_LINEARIZING_CONTROLLER_H
+
+#include <cstddef>
+#include <vector>
+#include <string>
+
+#include <Eigen/Dense>
+#include <boost/scoped_ptr.hpp>
+
+#include <ros/node_handle.h>
+
+#include <std_msgs/Float64MultiArray.h>
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/tfMessage.h>
+
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <arc_odometry/diff_odometry.h>
+#include <linearizing_controllers_msgs/TwistMracLinearizingControllerStatus.h>
+
+namespace effort_controllers
+{
+ class TwistMracLinearizingController: public controller_interface::
+ Controller<hardware_interface::EffortJointInterface>
+ {
+ 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<hardware_interface::JointHandle> joints_;
+
+ boost::scoped_ptr<realtime_tools::RealtimePublisher
+ <linearizing_controllers_msgs::TwistMracLinearizingControllerStatus>
+ > status_publisher_ ;
+
+ boost::shared_ptr<realtime_tools::RealtimePublisher
+ <nav_msgs::Odometry> > odom_publisher_;
+ boost::shared_ptr<realtime_tools::RealtimePublisher
+ <tf::tfMessage> > 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<double,2> Alpha_;
+
+ void commandCB(const geometry_msgs::Twist &command);
+ void parametersCB(const std_msgs::Float64MultiArray &command);
+ };
+}
+#endif
--- /dev/null
+<launch>
+ <node name="ident" pkg="twil_ident" type="ident" output="screen">
+ <remap from="ident/dynamic_parameters" to="adaptive_linearizing_controller/dynamic_parameters"/>
+ </node>
+</launch>
--- /dev/null
+<launch>
+ <rosparam file="$(find linearizing_controllers)/config/dynamics_linearizing_controller.yaml" command="load"/>
+
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" args="joint_state_controller dynamics_linearizing_controller"/>
+</launch>
--- /dev/null
+<launch>
+ <arg name="paused" default="true"/>
+ <arg name="headless" default="false"/>
+ <arg name="use_sim_time" default="true"/>
+ <arg name="wam" default="false"/>
+ <arg name="controller" default="twist_mrac"/>
+
+ <include file="$(find twil_description)/launch/gazebo.launch" >
+ <arg name="paused" value="$(arg paused)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+ <arg name="wam" value="$(arg wam)"/>
+ </include>
+
+ <include file="$(find linearizing_controllers)/launch/$(arg controller).launch" />
+
+</launch>
--- /dev/null
+<launch>
+ <rosparam file="$(find linearizing_controllers)/config/twist_mrac_linearizing_controller.yaml" command="load"/>
+
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" args="joint_state_controller twist_mrac_linearizing_controller"/>
+</launch>
--- /dev/null
+<library path="lib/liblinearizing_controllers">
+
+ <class name="effort_controllers/DynamicsLinearizingController"
+ type="effort_controllers::DynamicsLinearizingController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ 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.
+ </description>
+ </class>
+
+ <class name="effort_controllers/TwistMracLinearizingController"
+ type="effort_controllers::TwistMracLinearizingController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ 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.
+ </description>
+ </class>
+
+</library>
+
+ <class name="effort_controllers/PositionMracLinearizingController"
+ type="effort_controllers::PositionMracLinearizingController"
+ base_class_type="controller_interface::ControllerBase">
+ <description>
+ 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.
+ </description>
+ </class>
+
+</library>
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>linearizing_controllers</name>
+ <version>0.0.0</version>
+ <description>The linearizing_controllers package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/linearizing_controllers</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>Eigen</build_depend>
+ <build_depend>controller_interface</build_depend>
+ <build_depend>effort_controllers</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>arc_odometry</build_depend>
+ <build_depend>linearizing_controllers_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <run_depend>Eigen</run_depend>
+ <run_depend>controller_interface</run_depend>
+ <run_depend>effort_controllers</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>arc_odometry</run_depend>
+ <run_depend>linearizing_controllers_msgs</run_depend>
+ <run_depend>roscpp</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <controller_interface plugin="${prefix}/linearizing_controllers_plugins.xml"/>
+ </export>
+</package>
--- /dev/null
+#!/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
--- /dev/null
+#!/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}}"
--- /dev/null
+#!/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
--- /dev/null
+#!/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}}"
--- /dev/null
+/******************************************************************************
+ ROS linearing_controllers Package
+ Dynamics Linearizing Controller
+ Copyright (C) 2014..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <sys/mman.h>
+
+#include <boost/assign.hpp>
+
+#include <pluginlib/class_list_macros.h>
+
+#include <linearizing_controllers/dynamics_linearizing_controller.h>
+
+#define sqr(x) (x*x)
+
+namespace effort_controllers
+{
+ DynamicsLinearizingController::DynamicsLinearizingController(void):
+ odom_(0.0,std::vector<double>(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<linearizing_controllers_msgs::DynamicsLinearizingControllerStatus>(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<nav_msgs::Odometry>(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<tf::tfMessage>(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<double> 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<double> FVec;
+ if(!node_.getParam("F",FVec))
+ {
+ ROS_ERROR("No 'F' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ F_=Eigen::Map<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
+
+ std::vector<double> GVec;
+ if(!node_.getParam("G",GVec))
+ {
+ ROS_ERROR("No 'G' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ Ginv_=Eigen::Map<Eigen::Matrix2d>(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)
--- /dev/null
+/******************************************************************************
+ ROS linearing_controllers Package
+ Twist MRAC Linearizing Controller
+ Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <sys/mman.h>
+
+#include <boost/assign.hpp>
+
+#include <pluginlib/class_list_macros.h>
+
+#include <linearizing_controllers/twist_mrac_linearizing_controller.h>
+
+#define sqr(x) (x*x)
+
+namespace effort_controllers
+{
+ TwistMracLinearizingController::TwistMracLinearizingController(void):
+ odom_(0.0,std::vector<double>(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<linearizing_controllers_msgs::TwistMracLinearizingControllerStatus>(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<nav_msgs::Odometry>(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<tf::tfMessage>(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<double> 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<double> FVec;
+ if(!node_.getParam("F",FVec))
+ {
+ ROS_ERROR("No 'F' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ F_=Eigen::Map<Eigen::Matrix2d>(FVec.data(),2,2).transpose();
+
+ std::vector<double> GVec;
+ if(!node_.getParam("G",GVec))
+ {
+ ROS_ERROR("No 'G' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ Ginv_=Eigen::Map<Eigen::Matrix2d>(GVec.data(),2,2).transpose().inverse();
+
+ std::vector<double> AlphaVec;
+ if(!node_.getParam("Alpha",AlphaVec))
+ {
+ ROS_ERROR("No 'Alpha' in controller %s.",node_.getNamespace().c_str());
+ return false;
+ }
+ Alpha_.diagonal()=Eigen::Map<Eigen::Vector2d>(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)
projects / linearizing_controllers.git / commitdiff