From: Walter Fetter Lages Date: Wed, 21 Mar 2018 15:38:20 +0000 (-0300) Subject: Ported to Indigo X-Git-Tag: 2.0.0^0 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=dd2b6c67fa098c7f7871e872c5626184517c6d8c;p=twil.git Ported to Indigo --- diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 28105dd..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of -# directories (or patterns, but directories should suffice) that should -# be excluded from the distro. This is not the place to put things that -# should be ignored everywhere, like "build" directories; that happens in -# rosbuild/rosbuild.cmake. Here should be listed packages that aren't -# ready for inclusion in a distro. -# -# This list is combined with the list in rosbuild/rosbuild.cmake. Note -# that CMake 2.6 may be required to ensure that the two lists are combined -# properly. CMake 2.4 seems to have unpredictable scoping rules for such -# variables. -#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) - -rosbuild_make_distribution(0.1.0) diff --git a/Makefile b/Makefile deleted file mode 100644 index a818cca..0000000 --- a/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/stack.xml b/stack.xml deleted file mode 100644 index dd41a6b..0000000 --- a/stack.xml +++ /dev/null @@ -1,9 +0,0 @@ - - TWIL - Maintained by Taiser Barros - GPL - - http://ros.org/wiki/twil - - - diff --git a/twil/CMakeLists.txt b/twil/CMakeLists.txt new file mode 100644 index 0000000..ccd3cdb --- /dev/null +++ b/twil/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/twil/package.xml b/twil/package.xml new file mode 100644 index 0000000..3ccfd15 --- /dev/null +++ b/twil/package.xml @@ -0,0 +1,59 @@ + + + twil + 2.0.0 + The twil package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + http://www.ece.ufrgs.br/~fetter/twil + + + + + + Walter Fetter Lages + + + + + + + + + + + + + catkin + + twil_description + twil_gazebo_ros_control + twil_ident + twil_controllers + + + + + + + + + + + + \ No newline at end of file diff --git a/twil_control_gazebo/CMakeLists.txt b/twil_control_gazebo/CMakeLists.txt deleted file mode 100644 index 0796ff6..0000000 --- a/twil_control_gazebo/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -rosbuild_init() - -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) - -# Build RobotSim Interface -rosbuild_add_library(twil_control_gazebo src/robot_sim_twil.cpp) diff --git a/twil_control_gazebo/Makefile b/twil_control_gazebo/Makefile deleted file mode 100644 index b75b928..0000000 --- a/twil_control_gazebo/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/twil_control_gazebo/mainpage.dox b/twil_control_gazebo/mainpage.dox deleted file mode 100644 index 590fcdc..0000000 --- a/twil_control_gazebo/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b twil_control_gazebo - - - ---> - - -*/ diff --git a/twil_control_gazebo/manifest.xml b/twil_control_gazebo/manifest.xml deleted file mode 100644 index c3790a8..0000000 --- a/twil_control_gazebo/manifest.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - twil_control_gazebo - - - Walter Fetter Lages - GPL - - http://ros.org/wiki/twil_control_gazebo - - - - - - - - - - - - - diff --git a/twil_control_gazebo/robot_sim_plugins.xml b/twil_control_gazebo/robot_sim_plugins.xml deleted file mode 100644 index 9d6b5f8..0000000 --- a/twil_control_gazebo/robot_sim_plugins.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - A ROS/Gazebo interface Twil, exporting a joint_state_interface and a - joint_effort_interface. - - - diff --git a/twil_control_gazebo/src/robot_sim_twil.cpp b/twil_control_gazebo/src/robot_sim_twil.cpp deleted file mode 100644 index e88b33c..0000000 --- a/twil_control_gazebo/src/robot_sim_twil.cpp +++ /dev/null @@ -1,97 +0,0 @@ -#include - -#include -#include - -#include - -#include - -#include -#include -#include - -namespace twil_control_gazebo -{ - - class RobotSimTwil:public ros_control_gazebo::RobotSim - { - - unsigned int n_dof_; - - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - - std::vector joint_name_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_effort_command_; - - std::vector sim_joints_; - - public: - - RobotSimTwil(void):n_dof_(2),joint_name_(n_dof_),joint_position_(n_dof_), - joint_velocity_(n_dof_),joint_effort_(n_dof_),joint_effort_command_(n_dof_) - { - - joint_name_[0]="left_wheel_joint"; - joint_name_[1]="right_wheel_joint"; - - for(unsigned int j=0;j < n_dof_;j++) - { - joint_position_[j]=0.0; - joint_velocity_[j]=0.0; - joint_effort_[j]=0.0; - - joint_effort_command_[j] = 0.0; - - js_interface_.registerJoint(joint_name_[j],&joint_position_[j],&joint_velocity_[j],&joint_effort_[j]); - ej_interface_.registerJoint(js_interface_.getJointStateHandle(joint_name_[j]),&joint_effort_command_[j]); - } - - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - } - - - bool initSim(ros::NodeHandle nh,gazebo::physics::ModelPtr model) - { - for(unsigned int j=0;j < n_dof_;j++) - { - ROS_INFO_STREAM("Getting pointer to gazebo joint: " << joint_name_[j]); - gazebo::physics::JointPtr joint=model->GetJoint(joint_name_[j]); - if(joint) sim_joints_.push_back(joint); - else - { - ROS_ERROR_STREAM("This robot has a joint named \"" << joint_name_[j] - <<"\" which is not in the gazebo model."); - return false; - } - } - return true; - } - - void readSim(ros::Time time,ros::Duration period) - { - for(unsigned int j=0; j < n_dof_;j++) - { -// joint_position_[j]+=angles::shortest_angular_distance -// (joint_position_[j],sim_joints_[j]->GetAngle(0).GetAsRadian()); - joint_position_[j]=sim_joints_[j]->GetAngle(0).GetAsRadian(); - joint_velocity_[j]=sim_joints_[j]->GetVelocity(0); -// joint_effort_[j]=sim_joints_[j]->GetForce(0); - joint_effort_[j]=joint_effort_command_[j]; - } - } - - void writeSim(ros::Time time,ros::Duration period) - { - for(unsigned int j=0;j < n_dof_;j++) sim_joints_[j]->SetForce(0,joint_effort_command_[j]); - } - - }; -} - -PLUGINLIB_DECLARE_CLASS(twil_control_gazebo,RobotSimTwil,twil_control_gazebo::RobotSimTwil,ros_control_gazebo::RobotSim) diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt index 2318003..7261bb6 100644 --- a/twil_controllers/CMakeLists.txt +++ b/twil_controllers/CMakeLists.txt @@ -1,35 +1,166 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -rosbuild_init() - -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -rosbuild_add_library(${PROJECT_NAME} src/cart_linearizing_controller.cpp) -rosbuild_add_library(${PROJECT_NAME} src/cart_linearizing_controller_rne.cpp) -rosbuild_add_library(${PROJECT_NAME} src/nonsmooth_backstep_controller.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) +cmake_minimum_required(VERSION 2.8.3) +project(twil_controllers) +## 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 +) +find_package(cmake_modules REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) find_package(Eigen REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) + +## 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 and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * 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 +# ) + +################################### +## 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 ${PROJECT_NAME} + CATKIN_DEPENDS controller_interface +# DEPENDS system_lib + DEPENDS eigen +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(twil_controllers + src/cart_linearizing_controller.cpp + src/cart_linearizing_controller_rne.cpp + src/nonsmooth_backstep_controller.cpp +) + +## 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_node twil_controllers_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # 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_twil_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/twil_controllers/Makefile b/twil_controllers/Makefile deleted file mode 100644 index b75b928..0000000 --- a/twil_controllers/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h index ca952ba..2ba297d 100644 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h +++ b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h @@ -23,11 +23,10 @@ namespace twil_controllers bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); void starting(const ros::Time& time); - void update(const ros::Time& time); + void update(const ros::Time& time,const ros::Duration& duration); private: ros::NodeHandle node_; - ros::Time last_time_; hardware_interface::EffortJointInterface *robot_; std::vector joints_; diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h index 7aa2911..20df5d3 100644 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h +++ b/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h @@ -26,11 +26,10 @@ namespace twil_controllers bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); void starting(const ros::Time& time); - void update(const ros::Time& time); + void update(const ros::Time& time,const ros::Duration& duration); private: ros::NodeHandle node_; - ros::Time last_time_; hardware_interface::EffortJointInterface *robot_; std::vector joints_; diff --git a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h index 6dbbe98..4b9c205 100644 --- a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h +++ b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h @@ -23,11 +23,10 @@ namespace twil_controllers bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); void starting(const ros::Time& time); - void update(const ros::Time& time); + void update(const ros::Time& time,const ros::Duration& duration); private: ros::NodeHandle node_; - ros::Time last_time_; hardware_interface::EffortJointInterface *robot_; std::vector joints_; diff --git a/twil_controllers/mainpage.dox b/twil_controllers/mainpage.dox deleted file mode 100644 index deb448e..0000000 --- a/twil_controllers/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b twil_controllers - - - ---> - - -*/ diff --git a/twil_controllers/manifest.xml b/twil_controllers/manifest.xml deleted file mode 100644 index 9c40ae9..0000000 --- a/twil_controllers/manifest.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - twil_controllers - - - Walter Fetter Lages - GPL - - http://ros.org/wiki/twil_controllers - - - - - - - - - - - - - - diff --git a/twil_controllers/package.xml b/twil_controllers/package.xml new file mode 100644 index 0000000..81a7744 --- /dev/null +++ b/twil_controllers/package.xml @@ -0,0 +1,68 @@ + + + twil_controllers + 2.0.0 + The twil_controllers package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + + controller_interface + effort_controllers + kdl_parser + orocos_kdl + twil_gazebo_ros_control + + controller_interface + controller_manager + effort_controllers + joint_state_controller + kdl_parser + orocos_kdl + twil_gazebo_ros_control + + + + + + + + + + + \ No newline at end of file diff --git a/twil_controllers/src/cart_linearizing_controller.cpp b/twil_controllers/src/cart_linearizing_controller.cpp index 4080ee5..b411e7f 100644 --- a/twil_controllers/src/cart_linearizing_controller.cpp +++ b/twil_controllers/src/cart_linearizing_controller.cpp @@ -43,7 +43,7 @@ namespace twil_controllers return false; } - hardware_interface::JointHandle j=robot->getJointHandle((std::string)name_value); + hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); joints_.push_back(j); } sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,this); @@ -98,14 +98,12 @@ namespace twil_controllers void CartLinearizingController::starting(const ros::Time& time) { - last_time_=time; v.setZero(); } - void CartLinearizingController::update(const ros::Time& time) + void CartLinearizingController::update(const ros::Time& time, + const ros::Duration& duration) { - ros::Duration dt=time-last_time_; - last_time_=time; for(unsigned int i=0;i < joints_.size();i++) { @@ -130,4 +128,4 @@ namespace twil_controllers for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i]; } } -PLUGINLIB_DECLARE_CLASS(twil_controllers,CartLinearizingController,twil_controllers::CartLinearizingController,controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(twil_controllers::CartLinearizingController,controller_interface::ControllerBase) diff --git a/twil_controllers/src/cart_linearizing_controller_rne.cpp b/twil_controllers/src/cart_linearizing_controller_rne.cpp index 3190653..31fc7e7 100644 --- a/twil_controllers/src/cart_linearizing_controller_rne.cpp +++ b/twil_controllers/src/cart_linearizing_controller_rne.cpp @@ -44,7 +44,7 @@ namespace twil_controllers return false; } - hardware_interface::JointHandle j=robot->getJointHandle((std::string)name_value); + hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); joints_.push_back(j); v.push_back(0); } @@ -118,15 +118,12 @@ namespace twil_controllers void CartLinearizingController_RNE::starting(const ros::Time& time) { - last_time_=time; for(unsigned int i=0; i < joints_.size();i++) v[i]=0.0; } - void CartLinearizingController_RNE::update(const ros::Time& time) + void CartLinearizingController_RNE::update(const ros::Time& time, + const ros::Duration& duration) { - ros::Duration dt=time-last_time_; - last_time_=time; - for(unsigned int i=0;i < joints_.size();i++) { phi(i)=joints_[i].getPosition(); @@ -159,4 +156,4 @@ namespace twil_controllers for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i]; } } -PLUGINLIB_DECLARE_CLASS(twil_controllers,CartLinearizingController_RNE,twil_controllers::CartLinearizingController_RNE,controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(twil_controllers::CartLinearizingController_RNE,controller_interface::ControllerBase) diff --git a/twil_controllers/src/nonsmooth_backstep_controller.cpp b/twil_controllers/src/nonsmooth_backstep_controller.cpp index 0a37a7f..a5efb91 100644 --- a/twil_controllers/src/nonsmooth_backstep_controller.cpp +++ b/twil_controllers/src/nonsmooth_backstep_controller.cpp @@ -48,7 +48,7 @@ namespace twil_controllers return false; } - hardware_interface::JointHandle j=robot->getJointHandle((std::string)name_value); + hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); joints_.push_back(j); } sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this); @@ -104,18 +104,16 @@ namespace twil_controllers void NonSmoothBackstepController::starting(const ros::Time& time) { - last_time_=time; xi.setZero(); xiRef.setZero(); eta.setZero(); } - void NonSmoothBackstepController::update(const ros::Time& time) + void NonSmoothBackstepController::update(const ros::Time& time, + const ros::Duration& duration) { - ros::Duration dt=time-last_time_; - if(dt.toSec() < 0.01) return; - last_time_=time; + if(duration.toSec() < 0.01) return; Eigen::Vector2d nu; for(unsigned int i=0;i < joints_.size();i++) @@ -129,11 +127,11 @@ namespace twil_controllers // Estimate pose by odometry Eigen::MatrixXd B(3,2); - B << cos(xi[2]+u[1]*dt.toSec()/2.0), 0.0, - sin(xi[2]+u[1]*dt.toSec()/2.0), 0.0, + B << cos(xi[2]+u[1]*duration.toSec()/2.0), 0.0, + sin(xi[2]+u[1]*duration.toSec()/2.0), 0.0, 0.0, 1.0; - xi+=B*u*dt.toSec(); + xi+=B*u*duration.toSec(); // Change of coordinates Eigen::Matrix3d R; @@ -191,7 +189,7 @@ namespace twil_controllers +sqr(GAMMA1*LAMBDA3/LAMBDA2*cos(alpha)*sin(alpha)*psi)/cub(alpha); #else deta+=eta; - deta/=dt.toSec(); + deta/=duration.toSec(); #endif Eigen::Vector2d v=vBar+deta; @@ -242,4 +240,4 @@ namespace twil_controllers } -PLUGINLIB_DECLARE_CLASS(twil_controllers,NonSmoothBackstepController,twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase) diff --git a/twil_description/CMakeLists.txt b/twil_description/CMakeLists.txt index f8f1c9c..b28a957 100644 --- a/twil_description/CMakeLists.txt +++ b/twil_description/CMakeLists.txt @@ -1,30 +1,156 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -rosbuild_init() - -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) +cmake_minimum_required(VERSION 2.8.3) +project(twil_description) + +## 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) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * 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 +# ) + +################################### +## 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 twil_description +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +## Declare a cpp library +# add_library(twil_description +# src/${PROJECT_NAME}/twil_description.cpp +# ) + +## Declare a cpp executable +# add_executable(twil_description_node src/twil_description_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_description_node twil_description_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(twil_description_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS twil_description twil_description_node +# 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_twil_description.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/twil_description/Makefile b/twil_description/Makefile deleted file mode 100644 index b75b928..0000000 --- a/twil_description/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/twil_description/launch/twil.launch b/twil_description/launch/twil.launch index 384f343..bb29d83 100644 --- a/twil_description/launch/twil.launch +++ b/twil_description/launch/twil.launch @@ -1,4 +1,4 @@ - + diff --git a/twil_description/launch/twil_sim.launch b/twil_description/launch/twil_sim.launch index 58e959c..12f93b7 100644 --- a/twil_description/launch/twil_sim.launch +++ b/twil_description/launch/twil_sim.launch @@ -3,8 +3,9 @@ - + + diff --git a/twil_description/launch/twil_wam.launch b/twil_description/launch/twil_wam.launch index 1abdf97..a86b321 100644 --- a/twil_description/launch/twil_wam.launch +++ b/twil_description/launch/twil_wam.launch @@ -1,4 +1,4 @@ - + diff --git a/twil_description/launch/twil_wam_sim.launch b/twil_description/launch/twil_wam_sim.launch index 28aa15a..0edb037 100644 --- a/twil_description/launch/twil_wam_sim.launch +++ b/twil_description/launch/twil_wam_sim.launch @@ -4,8 +4,9 @@ - + + diff --git a/twil_description/mainpage.dox b/twil_description/mainpage.dox deleted file mode 100644 index 40fed2b..0000000 --- a/twil_description/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b twil_description - - - ---> - - -*/ diff --git a/twil_description/manifest.xml b/twil_description/manifest.xml deleted file mode 100644 index d5b36a3..0000000 --- a/twil_description/manifest.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - twil_description - - - Walter Fetter Lages - GPL - - http://ros.org/wiki/twil_description - - - - diff --git a/twil_description/package.xml b/twil_description/package.xml new file mode 100644 index 0000000..ae98f35 --- /dev/null +++ b/twil_description/package.xml @@ -0,0 +1,54 @@ + + + twil_description + 2.0.0 + The twil_description package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + + + + + + + + + + + diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro index d4ef22d..ebc8381 100644 --- a/twil_description/xacro/twil.urdf.xacro +++ b/twil_description/xacro/twil.urdf.xacro @@ -1,26 +1,17 @@ - - - + - - - - - - - - - + + + + + + + + + @@ -105,13 +96,18 @@ - - twil - twil_control_gazebo/RobotSimTwil + + /twil + + + + + + + 0.001 - + + diff --git a/twil_description/xacro/twil_wam.urdf.xacro b/twil_description/xacro/twil_wam.urdf.xacro index 1c1e31c..d594217 100644 --- a/twil_description/xacro/twil_wam.urdf.xacro +++ b/twil_description/xacro/twil_wam.urdf.xacro @@ -1,9 +1,9 @@ - + - + @@ -13,7 +13,7 @@ - + diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt index c50e0b4..bb4fe69 100644 --- a/twil_ident/CMakeLists.txt +++ b/twil_ident/CMakeLists.txt @@ -1,38 +1,164 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) - -# Set the build type. Options are: -# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage -# Debug : w/ debug symbols, w/o optimization -# Release : w/o debug symbols, w/ optimization -# RelWithDebInfo : w/ debug symbols, w/ optimization -# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries -#set(ROS_BUILD_TYPE RelWithDebInfo) - -rosbuild_init() - -#set the default path for built executables to the "bin" directory -set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -#set the default path for built libraries to the "lib" directory -set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) - -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() - -#common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) -#target_link_libraries(${PROJECT_NAME} another_library) -#rosbuild_add_boost_directories() -#rosbuild_link_boost(${PROJECT_NAME} thread) -#rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) +cmake_minimum_required(VERSION 2.8.3) +project(twil_ident) +## 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) + +find_package(cmake_modules REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) find_package(Eigen REQUIRED) -include_directories(${Eigen_INCLUDE_DIRS}) -#include_directories(${EIGEN_INCLUDE_DIRS}) -rosbuild_add_executable(ident src/ident.cpp) +## 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 and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * 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 +# ) + +################################### +## 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 twil_ident +# CATKIN_DEPENDS other_catkin_pkg + DEPENDS eigen +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +# TODO: Check names of system library include directories (eigen) +include_directories( + ${Eigen_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(twil_ident +# src/${PROJECT_NAME}/twil_ident.cpp +# ) + +## Declare a cpp executable +# add_executable(twil_ident_node src/twil_ident_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_ident_node twil_ident_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(twil_ident_node +# ${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 twil_ident twil_ident_node +# 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_twil_ident.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/twil_ident/Makefile b/twil_ident/Makefile deleted file mode 100644 index b75b928..0000000 --- a/twil_ident/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/twil_ident/mainpage.dox b/twil_ident/mainpage.dox deleted file mode 100644 index bdc871e..0000000 --- a/twil_ident/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b twil_ident - - - ---> - - -*/ diff --git a/twil_ident/manifest.xml b/twil_ident/manifest.xml deleted file mode 100644 index 109854c..0000000 --- a/twil_ident/manifest.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - twil_ident - - - Walter Fetter Lages - GPL - - http://ros.org/wiki/twil_ident - - - - - - - - diff --git a/twil_ident/package.xml b/twil_ident/package.xml new file mode 100644 index 0000000..56efcfa --- /dev/null +++ b/twil_ident/package.xml @@ -0,0 +1,56 @@ + + + twil_ident + 2.0.0 + The twil_ident package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + eigen + eigen + + + + + + + + + + +