From: Walter Fetter Lages Date: Mon, 26 Mar 2018 20:37:44 +0000 (-0300) Subject: Remove cart_linearizing_controller. X-Git-Tag: petryTCC~16 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=5d538f4a046f8424811bb6296cac39888f8feb05;p=twil.git Remove cart_linearizing_controller. --- diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt index cc194d7..c7f89f8 100644 --- a/twil_controllers/CMakeLists.txt +++ b/twil_controllers/CMakeLists.txt @@ -6,19 +6,13 @@ project(twil_controllers) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp - controller_interface effort_controllers - kdl_parser - geometry_msgs - nav_msgs - tf - message_generation ) find_package(cmake_modules REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(Eigen REQUIRED) +#find_package(Eigen REQUIRED) ## Uncomment this if the package has a setup.py. This macro ensures @@ -86,12 +80,12 @@ find_package(Eigen REQUIRED) ## 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 ${PROJECT_NAME} - CATKIN_DEPENDS message_runtime +#catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ${PROJECT_NAME} +# CATKIN_DEPENDS message_runtime # DEPENDS system_lib -) +#) ########### ## Build ## @@ -99,27 +93,26 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -include_directories(include - ${catkin_INCLUDE_DIRS} - ${Eigen_INCLUDE_DIRS} -) +#include_directories(include +# ${catkin_INCLUDE_DIRS} +# ${Eigen_INCLUDE_DIRS} +#) ## Declare a cpp library -add_library(twil_controllers - src/cart_linearizing_controller.cpp -) +#add_library(twil_controllers +#) ## Declare a cpp executable # add_executable(twil_controllers_node src/twil_controllers_node.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes -add_dependencies(twil_controllers twil_controllers_generate_messages_cpp) +#add_dependencies(twil_controllers twil_controllers_generate_messages_cpp) ## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) +#target_link_libraries(${PROJECT_NAME} +# ${catkin_LIBRARIES} +#) ############# ## Install ## @@ -136,18 +129,18 @@ target_link_libraries(${PROJECT_NAME} # ) ## 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} -) +#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} +#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 diff --git a/twil_controllers/README b/twil_controllers/README index 7c75eb7..72abf69 100644 --- a/twil_controllers/README +++ b/twil_controllers/README @@ -2,14 +2,6 @@ To publish with joint_effort_controller: rostopic pub /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" -To publish with cart_linearizing_controller: - -rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.1, 0.0]" - -To publish with nonsmmoth_backstep_controller: - -rostopic pub /twil/nonsmooth_backstep_controller/command geometry_ms/Pose2D '{x: 0.0, y: 0.0, theta: 0.0}' - To get time and pose: rostopic echo -p /gazebo/model_states | awk '{FS=","; printf("%g %g %g %g\n",$1/1e9,$11,$12,2*atan2(sqrt($14^2+$15^2+$16^2),$17))}' diff --git a/twil_controllers/config/linearizing_control.yaml b/twil_controllers/config/linearizing_control.yaml deleted file mode 100644 index a393a24..0000000 --- a/twil_controllers/config/linearizing_control.yaml +++ /dev/null @@ -1,11 +0,0 @@ -twil: - - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - - cart_linearizing_controller: - type: twil_controllers/CartLinearizingController - joints: - - left_wheel_joint - - right_wheel_joint diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h deleted file mode 100644 index 1ae6bc3..0000000 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h +++ /dev/null @@ -1,69 +0,0 @@ -/****************************************************************************** - ROS twil_controllers Package - Cartesian Linearizing Controller - Copyright (C) 2014..2017 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 TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H -#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_H - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -namespace twil_controllers -{ - class CartLinearizingController: public controller_interface::Controller - { - public: - CartLinearizingController(void); - ~CartLinearizingController(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_; - - void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); - ros::Subscriber sub_command_; - - Eigen::Vector2d nu; - Eigen::Vector2d torque; - Eigen::Vector2d v; - Eigen::Vector2d u; - - Eigen::Matrix2d Ginv; - Eigen::Matrix2d F; - - std::vector wheelRadius; - double wheelBase; - }; -} -#endif diff --git a/twil_controllers/launch/cart_linearizing.launch b/twil_controllers/launch/cart_linearizing.launch deleted file mode 100644 index f5089a8..0000000 --- a/twil_controllers/launch/cart_linearizing.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - diff --git a/twil_controllers/package.xml b/twil_controllers/package.xml index d1179cd..73ab19f 100644 --- a/twil_controllers/package.xml +++ b/twil_controllers/package.xml @@ -42,27 +42,11 @@ catkin - controller_interface effort_controllers - kdl_parser - geometry_msgs - nav_msgs - tf - control_toolbox - realtime_tools - message_generation - controller_interface controller_manager effort_controllers joint_state_controller - kdl_parser - geometry_msgs - nav_msgs - tf - control_toolbox - realtime_tools - message_runtime @@ -71,6 +55,5 @@ - diff --git a/twil_controllers/src/cart_linearizing_controller.cpp b/twil_controllers/src/cart_linearizing_controller.cpp deleted file mode 100644 index f04a5cd..0000000 --- a/twil_controllers/src/cart_linearizing_controller.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/****************************************************************************** - ROS twil_controllers Package - Cartesian Linearizing Controller - Copyright (C) 2014..2017 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 - -namespace twil_controllers -{ - CartLinearizingController::CartLinearizingController(void): - wheelRadius(2) - { - } - - CartLinearizingController::~CartLinearizingController(void) - { - sub_command_.shutdown(); - } - - bool CartLinearizingController::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); - } - sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,this); - - std::string robot_desc_string; - if(!node_.getParam("/robot_description",robot_desc_string)) - { - ROS_ERROR("Could not find '/robot_description'."); - return false; - } - - KDL::Tree tree; - if (!kdl_parser::treeFromString(robot_desc_string,tree)) - { - ROS_ERROR("Failed to construct KDL tree."); - return false; - } - - // get wheelBase from URDF (actually from KDL tree) - KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support"); - KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip(); - - segmentMapIter=tree.getSegment("right_wheel_support"); - KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip(); - - wheelRadius.resize(joints_.size()); - wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3); - - // get wheelRadius from URDF (actually from KDL tree) - segmentMapIter=tree.getSegment("chassis"); - KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip(); - - segmentMapIter=tree.getSegment("left_wheel"); - KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint(); - wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z(); - - segmentMapIter=tree.getSegment("right_wheel"); - KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); - wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); - - const double K5 = 0.0018533548425194695; - const double K6 = 0.09946140462774823; - const double K7 = 21.65458426501294; - const double K8 = -15.40102896939387; - Ginv << 0.5*K7, 0.5*K8, - 0.5*K7, -0.5*K8; - F << 0.0, K5, - K6, 0.0; - - return true; - } - - void CartLinearizingController::starting(const ros::Time& time) - { - v.setZero(); - } - - void CartLinearizingController::update(const ros::Time& time, - const ros::Duration& duration) - { - - for(unsigned int i=0;i < joints_.size();i++) - { - nu[i]=joints_[i].getVelocity(); - } - - u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0; - u[1]=(nu[0]*wheelRadius[0]-nu[1]*wheelRadius[1])/wheelBase; - - // Compute linearization - Eigen::Vector2d uf; - uf << u[0]*u[1], u[1]*u[1]; - - torque=Ginv*(v+F*uf); - - // Apply torques - for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i)); - } - - void CartLinearizingController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command) - { - for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i]; - } -} -PLUGINLIB_EXPORT_CLASS(twil_controllers::CartLinearizingController,controller_interface::ControllerBase) diff --git a/twil_controllers/twil_controllers_plugins.xml b/twil_controllers/twil_controllers_plugins.xml deleted file mode 100644 index a2346e0..0000000 --- a/twil_controllers/twil_controllers_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - The CartLinearizingController linearizes the Twil dynamic model. The - linearized inputs are linear and angular accelerations. It expects a - EffortJointInterface type of hardware interface. - - - -