From: Walter Fetter Lages Date: Wed, 21 Mar 2018 22:03:20 +0000 (-0300) Subject: add twil_ident X-Git-Tag: rosbook2018review^0 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=b654176fc9e535b1901edd4bb45d486d389fa4c4;p=twil.git add twil_ident fix wheel names remove twil_trajectories remove twil_controllers --- diff --git a/twil/package.xml b/twil/package.xml index 5d03e91..2d4a31b 100644 --- a/twil/package.xml +++ b/twil/package.xml @@ -45,11 +45,7 @@ - twil_controllers twil_ident - diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt deleted file mode 100644 index 5fd9edb..0000000 --- a/twil_controllers/CMakeLists.txt +++ /dev/null @@ -1,176 +0,0 @@ -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 - 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) - - -## 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 - NonSmoothBackstepControllerStatus.msg - PosePolar.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs # Or other packages containing msgs - geometry_msgs - nav_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 message_runtime -# DEPENDS system_lib -) - -########### -## 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 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/config/effort_control.yaml b/twil_controllers/config/effort_control.yaml deleted file mode 100644 index bdca9e8..0000000 --- a/twil_controllers/config/effort_control.yaml +++ /dev/null @@ -1,13 +0,0 @@ -twil: - - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - - left_wheel_joint_effort_controller: - type: effort_controllers/JointEffortController - joint: left_wheel_joint - - right_wheel_joint_effort_controller: - type: effort_controllers/JointEffortController - joint: right_wheel_joint 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/config/nonsmooth_backstep_control.yaml b/twil_controllers/config/nonsmooth_backstep_control.yaml deleted file mode 100644 index 6ff08e6..0000000 --- a/twil_controllers/config/nonsmooth_backstep_control.yaml +++ /dev/null @@ -1,15 +0,0 @@ -# Watch-out: The indentation here is relevant to the semantic! - -twil: - - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - - nonsmooth_backstep_controller: - type: twil_controllers/NonSmoothBackstepController - joints: - - left_wheel_joint - - right_wheel_joint - lambda: [10.0, 0.1, 0.1, 50.0, 100.0] - gamma: [10.0, 1.0, 10.0, 10.0] diff --git a/twil_controllers/config/velocity_control.yaml b/twil_controllers/config/velocity_control.yaml deleted file mode 100644 index 54aa494..0000000 --- a/twil_controllers/config/velocity_control.yaml +++ /dev/null @@ -1,16 +0,0 @@ -twil: - - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - - left_wheel_joint_velocity_controller: - type: effort_controllers/JointVelocityController - joint: left_wheel_joint - pid: {p: 0.0, i: 0.0, d: 0.0} - - right_wheel_joint_velocity_controller: - type: effort_controllers/JointVelocityController - joint: right_wheel_joint - pid: {p: 0.0, i: 0.0, d: 0.0} - 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 2ba297d..0000000 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h +++ /dev/null @@ -1,48 +0,0 @@ -#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/include/twil_controllers/nonsmooth_backstep_controller.h b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h deleted file mode 100644 index f37692e..0000000 --- a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H -#define TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace twil_controllers -{ - class NonSmoothBackstepController: public controller_interface:: - Controller - { - public: - NonSmoothBackstepController(void); - ~NonSmoothBackstepController(void); - - bool init(hardware_interface::EffortJointInterface *robot, - ros::NodeHandle &n); - void starting(const ros::Time& time); - void update(const ros::Time& time,const ros::Duration& duration); - - private: - ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; - std::vector joints_; - - boost::scoped_ptr - > status_publisher_ ; - - boost::shared_ptr > odom_publisher_; - boost::shared_ptr > tf_odom_publisher_; - - ros::Subscriber sub_command_; - ros::Subscriber sub_parameters_; - - Eigen::Matrix2d Ginv; - Eigen::Matrix2d F; - - std::vector wheelRadius; - double wheelBase; - - Eigen::Vector3d x; - Eigen::Vector3d xRef; - - Eigen::Vector2d eta; - - ros::Time lastSamplingTime; - - Eigen::Vector2d phi; - - std::vector lambda; - std::vector gamma; - - void commandCB(const geometry_msgs::Pose2D::ConstPtr &command); - void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); - }; -} -#endif diff --git a/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/launch/gazebo_joint_effort.launch b/twil_controllers/launch/gazebo_joint_effort.launch deleted file mode 100644 index 67f846b..0000000 --- a/twil_controllers/launch/gazebo_joint_effort.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch deleted file mode 100644 index 6375e93..0000000 --- a/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/twil_controllers/launch/joint_effort.launch b/twil_controllers/launch/joint_effort.launch deleted file mode 100644 index 222450c..0000000 --- a/twil_controllers/launch/joint_effort.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - diff --git a/twil_controllers/launch/joint_velocity.launch b/twil_controllers/launch/joint_velocity.launch deleted file mode 100644 index b93a84a..0000000 --- a/twil_controllers/launch/joint_velocity.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - diff --git a/twil_controllers/launch/nonsmooth_backstep.launch b/twil_controllers/launch/nonsmooth_backstep.launch deleted file mode 100644 index 695b6e3..0000000 --- a/twil_controllers/launch/nonsmooth_backstep.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - diff --git a/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg b/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg deleted file mode 100644 index 3012416..0000000 --- a/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg +++ /dev/null @@ -1,16 +0,0 @@ -Header header -geometry_msgs/Pose2D set_point -geometry_msgs/Pose2D process_value -geometry_msgs/Pose2D process_value_dot -geometry_msgs/Pose2D error -float64 time_step -float64[2] command -float64[5] lambda -float64[4] gamma -PosePolar polar_error -float64[2] backstep_set_point -float64[2] backstep_set_point_dot -float64[2] backstep_process_value -float64[2] backstep_error -float64[2] backstep_command -float64[2] linear_dynamics_command diff --git a/twil_controllers/msg/PosePolar.msg b/twil_controllers/msg/PosePolar.msg deleted file mode 100644 index 14a00fa..0000000 --- a/twil_controllers/msg/PosePolar.msg +++ /dev/null @@ -1,3 +0,0 @@ -float64 range -float64 angle -float64 orientation diff --git a/twil_controllers/package.xml b/twil_controllers/package.xml deleted file mode 100644 index d1179cd..0000000 --- a/twil_controllers/package.xml +++ /dev/null @@ -1,76 +0,0 @@ - - - twil_controllers - 2.0.0 - The twil_controllers package - - - - - Walter Fetter Lages - - - - - - GPLv3 - - - - - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - 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 - - - - - - - - - - - diff --git a/twil_controllers/scripts/pose_step.sh b/twil_controllers/scripts/pose_step.sh deleted file mode 100755 index 4d8e303..0000000 --- a/twil_controllers/scripts/pose_step.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -rostopic pub -1 /twil/nonsmooth_backstep_controller/command geometry_msgs/Pose2D "{x: $1, y: $2, theta: $3}" diff --git a/twil_controllers/scripts/test_openloop.sh b/twil_controllers/scripts/test_openloop.sh deleted file mode 100755 index e568f67..0000000 --- a/twil_controllers/scripts/test_openloop.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash - -rostopic pub -1 /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.5" -rostopic pub -1 /twil/right_wheel_joint_effort_controller/command std_msgs/Float64 "0.5" -sleep 3 -rostopic pub -1 /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" -rostopic pub -1 /twil/right_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" diff --git a/twil_controllers/src/cart_linearizing_controller.cpp b/twil_controllers/src/cart_linearizing_controller.cpp deleted file mode 100644 index b411e7f..0000000 --- a/twil_controllers/src/cart_linearizing_controller.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#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/src/nonsmooth_backstep_controller.cpp b/twil_controllers/src/nonsmooth_backstep_controller.cpp deleted file mode 100644 index 626d207..0000000 --- a/twil_controllers/src/nonsmooth_backstep_controller.cpp +++ /dev/null @@ -1,396 +0,0 @@ -#include - -#include -#include -#include - -#include - -#define sqr(x) (x*x) -#define cub(x) (x*x*x) - -namespace twil_controllers -{ - NonSmoothBackstepController::NonSmoothBackstepController(void): - wheelRadius(2),lambda(5),gamma(4) - { - } - - NonSmoothBackstepController::~NonSmoothBackstepController(void) - { - sub_command_.shutdown(); - sub_parameters_.shutdown(); - } - - bool NonSmoothBackstepController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) - { - node_=n; - robot_=robot; - - XmlRpc::XmlRpcValue joint_names; - if(!node_.getParam("joints",joint_names)) - { - ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - for(int i=0; i < joint_names.size();i++) - { - XmlRpc::XmlRpcValue &name_value=joint_names[i]; - if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) - { - ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); - joints_.push_back(j); - } - - status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); - status_publisher_->msg_.header.frame_id="odom"; - - odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"odom",100)); - odom_publisher_->msg_.header.frame_id="odom"; - odom_publisher_->msg_.child_frame_id="twil"; - odom_publisher_->msg_.pose.pose.position.z=0; - odom_publisher_->msg_.pose.pose.orientation.x=0; - odom_publisher_->msg_.pose.pose.orientation.y=0; - - // Fake covariance - double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.pose.covariance=boost::assign::list_of - (pose_cov[0]) (0) (0) (0) (0) (0) - (0) (pose_cov[1]) (0) (0) (0) (0) - (0) (0) (pose_cov[2]) (0) (0) (0) - (0) (0) (0) (pose_cov[3]) (0) (0) - (0) (0) (0) (0) (pose_cov[4]) (0) - (0) (0) (0) (0) (0) (pose_cov[5]); - - odom_publisher_->msg_.twist.twist.linear.z=0; - odom_publisher_->msg_.twist.twist.angular.x=0; - odom_publisher_->msg_.twist.twist.angular.y=0; - - //Fake covariance - double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; - odom_publisher_->msg_.twist.covariance=boost::assign::list_of - (twist_cov[0]) (0) (0) (0) (0) (0) - (0) (twist_cov[1]) (0) (0) (0) (0) - (0) (0) (twist_cov[2]) (0) (0) (0) - (0) (0) (0) (twist_cov[3]) (0) (0) - (0) (0) (0) (0) (twist_cov[4]) (0) - (0) (0) (0) (0) (0) (twist_cov[5]); - - tf_odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"/tf",100)); - tf_odom_publisher_->msg_.transforms.resize(1); - tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; - tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; - tf_odom_publisher_->msg_.transforms[0].child_frame_id="twil_origin"; - tf_odom_publisher_->msg_.transforms[0].header.frame_id="odom"; - - sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this); - - sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,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(); - - gamma[0]=10.0; - gamma[1]=1.0; - gamma[2]=10.0; - gamma[3]=10.0; - node_.param("gamma",gamma,gamma); - - lambda[0]=10.0; - lambda[1]=0.1; - lambda[2]=0.1; - lambda[3]=50.0; - lambda[4]=100.0; - node_.param("lambda",lambda,lambda); - - const double K5=0.08444758509282763; - const double K6=3.770688129256381; - const double K7=2.6468901285322475; - const double K8=-16.084061415321404; - - Ginv << 0.5/K7, 0.5/K8, - 0.5/K7, -0.5/K8; - F << 0.0, K5, - K6, 0.0; - - return true; - } - - void NonSmoothBackstepController::starting(const ros::Time& time) - { - x.setZero(); - xRef.setZero(); - eta.setZero(); - lastSamplingTime=time; - for(unsigned int i=0;i < joints_.size();i++) - { - phi[i]=joints_[i].getPosition(); - } - } - - void NonSmoothBackstepController::update(const ros::Time& time, - const ros::Duration& duration) - { - double dt=time.toSec()-lastSamplingTime.toSec(); - - if(fabs(dt-0.01) > 0.001/2) return; - lastSamplingTime=time; - - // Incremental encoders sense angular displacement and - // not velocity - // phi[0] is the left wheel angular displacement - // phi[1] is the right wheel angular displacement - Eigen::Vector2d deltaPhi=-phi; - for(unsigned int i=0;i < joints_.size();i++) - { - phi[i]=joints_[i].getPosition(); - } - deltaPhi+=phi; - - // Estimate pose by odometry - Eigen::Vector2d deltaU; - deltaU << (deltaPhi[0]*wheelRadius[0]+deltaPhi[1]*wheelRadius[1])/2.0, - (deltaPhi[1]*wheelRadius[1]-deltaPhi[0]*wheelRadius[0])/wheelBase; - - double deltaS=(fabs(deltaU[1]) > 1e-10)? - deltaU[0]*sin(deltaU[1])/deltaU[1]:deltaU[0]; - - Eigen::Vector3d deltaX; - deltaX << deltaS*cos(x[2]+deltaU[1]/2.0), - deltaS*sin(x[2]+deltaU[1]/2.0), - deltaU[1]; - - x+=deltaX; - - Eigen::Vector2d u=deltaU/dt; - - // Change of coordinates - Eigen::Matrix3d R; - R << cos(xRef[2]), sin(xRef[2]), 0.0, - -sin(xRef[2]), cos(xRef[2]), 0.0, - 0.0, 0.0, 1.0; - Eigen::Vector3d xBar=R*(x-xRef); - - // Discontinuous transformation - double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); - double psi=atan2(xBar[1],xBar[0]); - double alpha=xBar[2]-psi; - - // Backstepping - eta[0]=-gamma[0]*e*cos(alpha); - - if(fabs(alpha ) > 1e-10) - eta[1]=-gamma[1]*alpha - -gamma[0]*sin(alpha)*cos(alpha)+gamma[0]*lambda[2]*psi*sin(alpha)/ - lambda[1]/alpha*cos(alpha); - else eta[1]=gamma[0]*lambda[2]*psi/lambda[1]; - - Eigen::Vector2d eb=u-eta; - - Eigen::Vector2d vBar; - if(fabs(e) > 1e-10) vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha) - +lambda[1]/lambda[3]*alpha*sin(alpha)/e - -lambda[2]/lambda[3]*psi*sin(alpha)/e; - else vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha); - vBar[1]=-gamma[3]*eb[1]-lambda[1]/lambda[4]*alpha; - - Eigen::Vector2d deta; - - deta[0]=e*cub(cos(alpha))*sqr(gamma[0])-e*gamma[0]*sin(alpha)*(alpha*gamma[1] - -(gamma[0]*lambda[2]*cos(alpha)*sin(alpha)*psi)/(alpha*lambda[1])); - - deta[1]=gamma[1]*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) - +gamma[0]*sqr(cos(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) - -gamma[0]*sqr(sin(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) - -(lambda[2]*sqr(cos(alpha))*sqr(sin(alpha))*sqr(gamma[0]))/(alpha*lambda[1]) - -(gamma[0]*lambda[2]*psi*sqr(cos(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) - /(alpha*lambda[1]) - +(gamma[0]*lambda[2]*psi*sqr(sin(alpha))*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) - /(alpha*lambda[1]) - +(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha)*(alpha*gamma[1] - -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) - /(sqr(alpha)*lambda[1]); - - Eigen::Vector2d v=vBar+deta; - - // Linearization - Eigen::Vector2d uf; - uf << u[0]*u[1], sqr(u[1]); - - Eigen::Vector2d torque=Ginv*(v-F*uf); - - // Apply torques - for(unsigned int i=0;i < joints_.size();i++) - { - joints_[i].setCommand(torque[i]); - } - - if(status_publisher_ && status_publisher_->trylock()) - { - status_publisher_->msg_.header.stamp=time; - - status_publisher_->msg_.set_point.x=xRef[0]; - status_publisher_->msg_.set_point.y=xRef[1]; - status_publisher_->msg_.set_point.theta=xRef[2]; - - status_publisher_->msg_.process_value.x=x[0]; - status_publisher_->msg_.process_value.y=x[1]; - status_publisher_->msg_.process_value.theta=x[2]; - - status_publisher_->msg_.process_value_dot.x=deltaX[0]/dt; - status_publisher_->msg_.process_value_dot.y=deltaX[1]/dt; - status_publisher_->msg_.process_value_dot.theta=deltaX[2]/dt; - - status_publisher_->msg_.error.x=xRef[0]-x[0]; - status_publisher_->msg_.error.y=xRef[1]-x[1]; - status_publisher_->msg_.error.theta=xRef[2]-x[2]; - - status_publisher_->msg_.time_step=dt; - - for(int i=0;i < torque.size();i++) - status_publisher_->msg_.command[i]=torque[i]; - - for(int i=0;i < lambda.size();i++) - status_publisher_->msg_.lambda[i]=lambda[i]; - - for(int i=0;i < gamma.size();i++) - status_publisher_->msg_.gamma[i]=gamma[i]; - - status_publisher_->msg_.polar_error.range=e; - status_publisher_->msg_.polar_error.angle=psi; - status_publisher_->msg_.polar_error.orientation=alpha; - - for(int i=0;i < eta.size();i++) - status_publisher_->msg_.backstep_set_point[i]=eta[i]; - - for(int i=0;i < deta.size();i++) - status_publisher_->msg_.backstep_set_point_dot[i]=deta[i]; - - for(int i=0;i < u.size();i++) - status_publisher_->msg_.backstep_process_value[i]=u[i]; - - for(int i=0;i < eb.size();i++) - status_publisher_->msg_.backstep_error[i]=eb[i]; - - for(int i=0;i < vBar.size();i++) - status_publisher_->msg_.backstep_command[i]=vBar[i]; - - for(int i=0;i < v.size();i++) - status_publisher_->msg_.linear_dynamics_command[i]=v[i]; - - status_publisher_->unlockAndPublish(); - } - - if(odom_publisher_ && odom_publisher_->trylock()) - { - odom_publisher_->msg_.header.stamp=time; - - odom_publisher_->msg_.pose.pose.position.x=x[0]; - odom_publisher_->msg_.pose.pose.position.y=x[1]; - odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); - odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); - - odom_publisher_->msg_.twist.twist.linear.x=deltaX[0]/dt; - odom_publisher_->msg_.twist.twist.linear.y=deltaX[1]/dt; - odom_publisher_->msg_.twist.twist.angular.z=deltaX[2]/dt; - - odom_publisher_->unlockAndPublish(); - } - - if(tf_odom_publisher_ && tf_odom_publisher_->trylock()) - { - geometry_msgs::TransformStamped &odom_frame= - tf_odom_publisher_->msg_.transforms[0]; - odom_frame.header.stamp=time; - odom_frame.transform.translation.x=x[0]; - odom_frame.transform.translation.y=x[1]; - odom_frame.transform.rotation.z=sin(x[2]/2); - odom_frame.transform.rotation.w=cos(x[2]/2); - - tf_odom_publisher_->unlockAndPublish(); - } - } - - void NonSmoothBackstepController::commandCB(const geometry_msgs::Pose2D::ConstPtr &command) - { - xRef[0]=command->x; - xRef[1]=command->y; - xRef[2]=command->theta; - } - - void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr ¶m) - { - // data[0]=K5, data[1]=K6, data[2]=K7, data[3]=K8 - // data[4]=Pk5, data[5]=Pk6, data[6]=Pk7, data[7]=Pk8 - // F= [0 K5 - // K6 0] - // Ginv=[0.5/K7 0.5/K8 - // 0.5/K7 -0.5/K8] - - 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(twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase) diff --git a/twil_controllers/twil_controllers_plugins.xml b/twil_controllers/twil_controllers_plugins.xml deleted file mode 100644 index 15684e6..0000000 --- a/twil_controllers/twil_controllers_plugins.xml +++ /dev/null @@ -1,31 +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. - - - - - - The CartLinearizingController linearizes the Twil dynamic model. The - linearized inputs are linear and angular accelerations. It expects a - EffortJointInterface type of hardware interface. Recursive - Newton-Euler from KDL is used to compute torque. Not working - properly. - - - - - - The NonSmoothBackstepController implements a non-smooth controller - based on a discontinuous transformation and backstepping. A feedback - linearization of the Twil dynamic model is also performed. The - inputs to the controller are the reference Cartesian pose. It expects a - EffortJointInterface type of hardware interface. - - - - diff --git a/twil_description/launch/display.launch b/twil_description/launch/display.launch index b17a754..86ab474 100644 --- a/twil_description/launch/display.launch +++ b/twil_description/launch/display.launch @@ -6,9 +6,11 @@ + + - + diff --git a/twil_description/meshes/castor_base.stl b/twil_description/meshes/caster_base.stl similarity index 100% rename from twil_description/meshes/castor_base.stl rename to twil_description/meshes/caster_base.stl diff --git a/twil_description/meshes/castor_support.stl b/twil_description/meshes/caster_support.stl similarity index 100% rename from twil_description/meshes/castor_support.stl rename to twil_description/meshes/caster_support.stl diff --git a/twil_description/meshes/castor_wheel.stl b/twil_description/meshes/caster_wheel.stl similarity index 100% rename from twil_description/meshes/castor_wheel.stl rename to twil_description/meshes/caster_wheel.stl diff --git a/twil_description/rviz/urdf.rviz b/twil_description/rviz/display.rviz similarity index 96% rename from twil_description/rviz/urdf.rviz rename to twil_description/rviz/display.rviz index c1db83d..a7b228d 100644 --- a/twil_description/rviz/urdf.rviz +++ b/twil_description/rviz/display.rviz @@ -65,17 +65,17 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - castor_base: + caster_base: Alpha: 1 Show Axes: false Show Trail: false Value: true - castor_support: + caster_support: Alpha: 1 Show Axes: false Show Trail: false Value: true - castor_wheel: + caster_wheel: Alpha: 1 Show Axes: false Show Trail: false @@ -99,17 +99,17 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - front_castor_base: + front_caster_base: Alpha: 1 Show Axes: false Show Trail: false Value: true - front_castor_support: + front_caster_support: Alpha: 1 Show Axes: false Show Trail: false Value: true - front_castor_wheel: + front_caster_wheel: Alpha: 1 Show Axes: false Show Trail: false @@ -167,7 +167,7 @@ Visualization Manager: Length: 1 Name: Odometry Position Tolerance: 0.3 - Topic: /twil/nonsmooth_backstep_controller/odom + Topic: /nonsmooth_backstep_controller/odom Value: true - Alpha: 1 Axes Length: 1 @@ -181,7 +181,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /twil/command_stamped + Topic: /command_stamped Value: true Enabled: true Global Options: diff --git a/twil_description/xacro/castor_base.urdf.xacro b/twil_description/xacro/caster_base.urdf.xacro similarity index 83% rename from twil_description/xacro/castor_base.urdf.xacro rename to twil_description/xacro/caster_base.urdf.xacro index 9271976..d38f6f6 100644 --- a/twil_description/xacro/castor_base.urdf.xacro +++ b/twil_description/xacro/caster_base.urdf.xacro @@ -2,7 +2,7 @@ - + @@ -13,7 +13,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/twil_description/xacro/castor_support.urdf.xacro b/twil_description/xacro/caster_support.urdf.xacro similarity index 83% rename from twil_description/xacro/castor_support.urdf.xacro rename to twil_description/xacro/caster_support.urdf.xacro index b5c86d0..dac2725 100644 --- a/twil_description/xacro/castor_support.urdf.xacro +++ b/twil_description/xacro/caster_support.urdf.xacro @@ -2,7 +2,7 @@ - + @@ -13,7 +13,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/twil_description/xacro/castor_wheel.urdf.xacro b/twil_description/xacro/caster_wheel.urdf.xacro similarity index 85% rename from twil_description/xacro/castor_wheel.urdf.xacro rename to twil_description/xacro/caster_wheel.urdf.xacro index e697875..a1aaf19 100644 --- a/twil_description/xacro/castor_wheel.urdf.xacro +++ b/twil_description/xacro/caster_wheel.urdf.xacro @@ -2,7 +2,7 @@ - + @@ -13,7 +13,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro index ebc8381..4d1321c 100644 --- a/twil_description/xacro/twil.urdf.xacro +++ b/twil_description/xacro/twil.urdf.xacro @@ -1,14 +1,14 @@ - + - - - + + + @@ -35,29 +35,29 @@ - + - + - + - + - + - + - + - + - + - + - + - + diff --git a/twil_description/xacro/twil_wam.urdf.xacro b/twil_description/xacro/twil_wam.urdf.xacro index d594217..9074f66 100644 --- a/twil_description/xacro/twil_wam.urdf.xacro +++ b/twil_description/xacro/twil_wam.urdf.xacro @@ -1,5 +1,5 @@ - + @@ -13,19 +13,19 @@ - + - + - + - + - + - + - + diff --git a/twil_trajectories/CMakeLists.txt b/twil_ident/CMakeLists.txt similarity index 82% rename from twil_trajectories/CMakeLists.txt rename to twil_ident/CMakeLists.txt index a38b1ef..f4103d8 100644 --- a/twil_trajectories/CMakeLists.txt +++ b/twil_ident/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) -project(twil_trajectories) +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 COMPONENTS roscpp) +find_package(catkin REQUIRED COMPONENTS roscpp kdl_parser) find_package(cmake_modules REQUIRED) @@ -79,8 +79,8 @@ find_package(Eigen REQUIRED) ## 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} +# INCLUDE_DIRS include +# LIBRARIES twil_ident # CATKIN_DEPENDS other_catkin_pkg DEPENDS eigen ) @@ -91,41 +91,29 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -# include_directories(include) +# include_directories( + include + ${catkin_INCLUDE_DIRS} # TODO: Check names of system library include directories (eigen) -include_directories( - include ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ) ## Declare a cpp library -add_library(${PROJECT_NAME} - src/circle_path.cpp - src/eight_path.cpp -) +# add_library(twil_ident +# src/${PROJECT_NAME}/twil_ident.cpp +# ) ## Declare a cpp executable -add_executable(eight_trajectory src/eight_trajectory.cpp) -add_executable(pose2d_stamp src/pose2d_stamp.cpp) +add_executable(ident src/ident.cpp) ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes -# add_dependencies(twil_trajectories_node twil_trajectories_generate_messages_cpp) +# add_dependencies(twil_ident_node twil_ident_generate_messages_cpp) ## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${eigen_LIBRARIES} -) - -target_link_libraries(eight_trajectory +target_link_libraries(ident ${catkin_LIBRARIES} ${eigen_LIBRARIES} - ${PROJECT_NAME} -) - -target_link_libraries(pose2d_stamp - ${catkin_LIBRARIES} ) ############# @@ -143,18 +131,18 @@ target_link_libraries(pose2d_stamp # ) ## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +install(TARGETS ident +# 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 @@ -168,7 +156,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_trajectories.cpp) +# 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() diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch b/twil_ident/launch/gazebo.launch similarity index 50% rename from twil_controllers/launch/gazebo_nonsmooth_backstep.launch rename to twil_ident/launch/gazebo.launch index 16002aa..a18f7f6 100644 --- a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch +++ b/twil_ident/launch/gazebo.launch @@ -2,17 +2,18 @@ - - + + + + + - - - + diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch new file mode 100644 index 0000000..e939edf --- /dev/null +++ b/twil_ident/launch/ident.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/twil_trajectories/package.xml b/twil_ident/package.xml similarity index 91% rename from twil_trajectories/package.xml rename to twil_ident/package.xml index 8545852..ffefa1b 100644 --- a/twil_trajectories/package.xml +++ b/twil_ident/package.xml @@ -1,8 +1,8 @@ - twil_trajectories + twil_ident 2.0.0 - The twil_trajectories package + The twil_ident package @@ -42,10 +42,9 @@ catkin eigen - geometry_msgs - + kdl_parser eigen - geometry_msgs + kdl_parser diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp new file mode 100644 index 0000000..88e6fb0 --- /dev/null +++ b/twil_ident/src/ident.cpp @@ -0,0 +1,263 @@ +/****************************************************************************** + ROS twil_indet Package + Twil Dynamics Model + 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 +#include + +#include + + +class Prbs +{ + int index; + unsigned int nd; + unsigned char *sh; + + public: + + Prbs(unsigned int n=10,unsigned int seed=0); + ~Prbs(void); + + void seed(unsigned int s) { index=s % nd; }; + + operator int(void); +}; + + +Prbs::Prbs(unsigned int n,unsigned int seed) +{ + nd=n; + index=seed % nd; + sh=new unsigned char[nd]; + for(unsigned int i=0; i < nd;i++) sh[i]=1; + for(unsigned int i=0; i < 98;i++) (int) *this; // call operator int() to exercise + +} + +Prbs::~Prbs(void) +{ +// delete[] sh; +} + +Prbs::operator int(void) +{ + unsigned char s=sh[nd-1]+sh[nd-2]; + if(s > 1) s=0; + for(int j=nd-2;j >= 0;j--) sh[j+1]=sh[j]; + sh[0]=s; + return sh[index]; +} + + +class Ident +{ + public: + Ident(ros::NodeHandle node); + ~Ident(void); + void setCommand(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber jointStatesSubscriber; + ros::Publisher dynParamPublisher; + ros::Publisher leftWheelCommandPublisher; + ros::Publisher rightWheelCommandPublisher; + + const int nJoints; + + Eigen::VectorXd u; + Eigen::VectorXd thetaEst1; + Eigen::VectorXd thetaEst2; + Eigen::MatrixXd P1; + Eigen::MatrixXd P2; + + std::vector prbs; + int iter; + + ros::Time lastTime; + + std::vector wheelRadius; + double wheelBase; + + void jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates); + void resetCovariance(void); +}; + + +Ident::Ident(ros::NodeHandle node): + nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints),wheelRadius(nJoints) +{ + node_=node; + + std::string robot_desc_string; + if(!node_.getParam("/robot_description",robot_desc_string)) + { + ROS_ERROR("Could not find '/robot_description'."); + } + + KDL::Tree tree; + if (!kdl_parser::treeFromString(robot_desc_string,tree)) + { + ROS_ERROR("Failed to construct KDL tree."); + } + + // 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(nJoints); + 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(); + + jointStatesSubscriber=node_.subscribe("joint_states",1000,&Ident::jointStatesCB,this); + dynParamPublisher=node_.advertise("ident/dynamic_parameters",1000); + leftWheelCommandPublisher=node_.advertise("ident/left_wheel_command",1000); + rightWheelCommandPublisher=node_.advertise("ident/right_wheel_command",1000); + + u.setZero(); + thetaEst1.setZero(); + thetaEst2.setZero(); + resetCovariance(); + + lastTime=ros::Time::now(); +} + +Ident::~Ident(void) +{ + jointStatesSubscriber.shutdown(); +} + +void Ident::resetCovariance(void) +{ + P1.setIdentity(); + P1*=1; + P2.setIdentity(); + P2*=1; + iter=0; +} + +void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) +{ + ros::Duration dt=jointStates->header.stamp-lastTime; + lastTime=jointStates->header.stamp; + + Eigen::VectorXd y=-u; //y(k+1)=(u(k+1)-u(k))/dt + + Eigen::VectorXd Phi1(nJoints); + Eigen::VectorXd Phi2(nJoints); + Phi1[0]=u[1]*u[1]; // u2^2(k) + Phi2[0]=u[0]*u[1]; // u1(k)*u2(k) + + Eigen::VectorXd torque(nJoints); + + // u(k+1) + // jointStates->velocity[0] is left wheel + // jointStates->velocity[1] is right wheel + u[0]=(jointStates->velocity[0]*wheelRadius[0]+jointStates->velocity[1]*wheelRadius[1])/2; + u[1]=(jointStates->velocity[1]*wheelRadius[1]-jointStates->velocity[0]*wheelRadius[0])/wheelBase; + + for(int i=0;i < nJoints;i++) + torque[i]=jointStates->effort[i]; // torque(k) + + y+=u; + y/=dt.toSec(); + + Phi1[1]=torque[0]+torque[1]; + Phi2[1]=torque[0]-torque[1]; + + double yEst1=Phi1.transpose()*thetaEst1; + Eigen::VectorXd K1=P1*Phi1/(1+Phi1.transpose()*P1*Phi1); + thetaEst1+=K1*(y[0]-yEst1); + P1-=K1*Phi1.transpose()*P1; + + double yEst2=Phi2.transpose()*thetaEst2; + Eigen::VectorXd K2=P2*Phi2/(1+Phi2.transpose()*P2*Phi2); + thetaEst2+=K2*(y[1]-yEst2); + P2-=K2*Phi2.transpose()*P2; + + std_msgs::Float64MultiArray dynParam; + dynParam.layout.dim.push_back(std_msgs::MultiArrayDimension()); + dynParam.layout.dim[0].label="K5 K6 K7 K8 P55 P66 P77 P88"; + dynParam.layout.dim[0].size=nJoints*4; + dynParam.layout.dim[0].stride=1; + dynParam.layout.data_offset=0; + for(int i=0;i < nJoints;i++) + { + dynParam.data.push_back(thetaEst1[i]); + dynParam.data.push_back(thetaEst2[i]); + } + for(int i=0;i < nJoints;i++) + { + dynParam.data.push_back(P1(i,i)); + dynParam.data.push_back(P2(i,i)); + } + dynParamPublisher.publish(dynParam); + +// if(iter++ > 2048) resetCovariance(); +} + +void Ident::setCommand(void) +{ + std_msgs::Float64 leftCommand; + std_msgs::Float64 rightCommand; + leftCommand.data=10.0*prbs[0]-5.0; + rightCommand.data=10.0*prbs[1]-5.0; + leftWheelCommandPublisher.publish(leftCommand); + rightWheelCommandPublisher.publish(rightCommand); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"twil_ident"); + ros::NodeHandle node; + + Ident ident(node); + + ros::Rate loop(100); + while(ros::ok()) + { + ident.setCommand(); + + ros::spinOnce(); + loop.sleep(); + } + return 0; +} diff --git a/twil_trajectories/include/twil_trajectories/circle_path.h b/twil_trajectories/include/twil_trajectories/circle_path.h deleted file mode 100644 index 4c16412..0000000 --- a/twil_trajectories/include/twil_trajectories/circle_path.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef CIRCLE_PATH_H -#define CIRCLE_PATH_H - -#include - -/** Circle path -* @author Walter Fetter Lages -*/ -class CirclePath -{ - Eigen::Vector2d pc_; - double phi0_; - double r_; - double w_; - - public: - - /** Build a circle path. - * @param pc circle center point. - * @param phi0 initial phase. - * @param r circle radius. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w); - - /** Build a circle path. - * @param p0 starting pose. - * @param r circle radius. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector3d &p0,double r,double w); - - /** Build a circle path. - * @param pc circle center point. - * @param p0 starting point. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w); - - /** Destroy a circle path. - */ - ~CirclePath(void) { }; - - /** Get path point. - * @param t path time. - * @return path point. - */ - Eigen::Vector3d point(double t) const; - - /** Get path steering controls. - * @param t path time. - * @return steering controls. - */ - Eigen::Vector2d steering(double t) const; -}; -#endif diff --git a/twil_trajectories/include/twil_trajectories/eight_path.h b/twil_trajectories/include/twil_trajectories/eight_path.h deleted file mode 100644 index b3f5af2..0000000 --- a/twil_trajectories/include/twil_trajectories/eight_path.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef EIGHT_PATH_H -#define EIGHT_PATH_H - -#include - -/** 8 path -* @author Walter Fetter Lages -*/ -class EightPath -{ - CirclePath c1_; - CirclePath c2_; - double period_; - - public: - - /** Build an 8 path. - * @param pc center point. - * @param r radius. - * @param w angular velocity. - */ - EightPath(const Eigen::Vector2d &pc,double r,double w); - - /** Destroy an 8 path. - */ - ~EightPath(void) { }; - - /** Get path point. - * @param t path time. - * @return path point. - */ - Eigen::Vector3d point(double t) const; - - /** Get path steering controls. - * @param t path time. - * @return steering controls. - */ - Eigen::Vector2d steering(double t) const; -}; -#endif diff --git a/twil_trajectories/src/circle_path.cpp b/twil_trajectories/src/circle_path.cpp deleted file mode 100644 index 8c7c805..0000000 --- a/twil_trajectories/src/circle_path.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include - -#define sqr(x) (x*x) -#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) - -CirclePath::CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w) -{ - pc_=pc; - phi0_=phi0; - r_=r; - w_=w; -} - -CirclePath::CirclePath(const Eigen::Vector3d &p0,double r,double w) -{ - phi0_=p0[2]-sgn(w)*M_PI_2; - pc_[0]=p0[0]-r*cos(phi0_); - pc_[1]=p0[1]-r*sin(phi0_); - r_=r; - w_=w; -} - -CirclePath::CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w) -{ - pc_=pc; - w_=w; - phi0_=atan2(p0[1]-pc[1],p0[0]-pc[0]); - r_=sqrt(sqr(p0[1]-pc[1])+sqr(p0[0]-pc[0])); -} - -Eigen::Vector3d CirclePath::point(double t) const -{ - double wt=w_*t; - Eigen::Vector3d p; - - p[0]=pc_[0]+r_*cos(wt+phi0_); - p[1]=pc_[1]+r_*sin(wt+phi0_); - p[2]=wt+phi0_+sgn(w_)*M_PI_2; - - return p; -} - -Eigen::Vector2d CirclePath::steering(double t) const -{ - Eigen::Vector2d e(2); - - e[0]=w_*r_; - e[1]=w_; - - return e; -} diff --git a/twil_trajectories/src/eight_path.cpp b/twil_trajectories/src/eight_path.cpp deleted file mode 100644 index 3a7603f..0000000 --- a/twil_trajectories/src/eight_path.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include - -#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) - -EightPath::EightPath(const Eigen::Vector2d &pc,double r,double w): -c1_(Eigen::Vector2d(pc[0],pc[1]+r),-M_PI_2*sgn(w),r,w), -c2_(Eigen::Vector2d(pc[0],pc[1]-r),2.5*M_PI*sgn(w),r,-w) -{ - period_=4*M_PI/w; -}; - -Eigen::Vector3d EightPath::point(double t) const -{ - double tc=fmod(t,period_/2.0); - - if(int(t/(period_/2.0)) % 2 == 0) return c1_.point(tc); - else return c2_.point(tc); -} - -Eigen::Vector2d EightPath::steering(double t) const -{ - double tc=fmod(t,period_/2.0); - - if(int(t/(period_/2.0)) % 2 == 0) return c1_.steering(tc); - else return c2_.steering(tc); -} diff --git a/twil_trajectories/src/eight_trajectory.cpp b/twil_trajectories/src/eight_trajectory.cpp deleted file mode 100644 index 6f0a059..0000000 --- a/twil_trajectories/src/eight_trajectory.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include -#include - -#include - -class EightTrajectory -{ - public: - EightTrajectory(ros::NodeHandle node); - ~EightTrajectory(void); - void setCommand(ros::Duration t); - - private: - ros::NodeHandle node_; - const EightPath *path; - - ros::Publisher commandPublisher; -}; - - -EightTrajectory::EightTrajectory(ros::NodeHandle node) -{ - node_=node; - Eigen::Vector2d pc; - double w; - double r; - - ros::param::get("~x",pc[0]); - ros::param::get("~y",pc[1]); - ros::param::get("~radius",r); - ros::param::get("~ang_vel",w); - - path=new EightPath(pc,r,w); - commandPublisher=node_.advertise("command",1000); -} - -EightTrajectory::~EightTrajectory(void) -{ - commandPublisher.shutdown(); - delete path; -} - -void EightTrajectory::setCommand(ros::Duration t) -{ - Eigen::Vector3d p=path->point(t.toSec()); - - geometry_msgs::Pose2D command; - command.x=p[0]; - command.y=p[1]; - command.theta=p[2]; - commandPublisher.publish(command); -} - -int main(int argc,char* argv[]) -{ - - ros::init(argc,argv,"eight_trajectory"); - ros::NodeHandle node; - - EightTrajectory eightTrajectory(node); - - ros::Rate loop(100); - - ros::Time t0=ros::Time::now(); - while(ros::ok()) - { - eightTrajectory.setCommand(ros::Time::now()-t0); - - ros::spinOnce(); - loop.sleep(); - } - return 0; -} diff --git a/twil_trajectories/src/pose2d_stamp.cpp b/twil_trajectories/src/pose2d_stamp.cpp deleted file mode 100644 index 84e0164..0000000 --- a/twil_trajectories/src/pose2d_stamp.cpp +++ /dev/null @@ -1,75 +0,0 @@ -#include - -#include - -#include -#include - -class Pose2DStamp -{ - public: - Pose2DStamp(ros::NodeHandle node); - ~Pose2DStamp(void); - - private: - ros::NodeHandle node_; - - ros::Subscriber poseSubscriber; - ros::Publisher posePublisher; - - int seq; - std::string frame_id; - - void poseCB(const geometry_msgs::Pose2D::ConstPtr &pose); -}; - - -Pose2DStamp::Pose2DStamp(ros::NodeHandle node) -{ - node_=node; - - poseSubscriber=node_.subscribe("command",1000,&Pose2DStamp::poseCB,this); - posePublisher=node_.advertise("command_stamped",1000); - - seq=0; - - ros::param::get("~frame_id",frame_id); -} - -Pose2DStamp::~Pose2DStamp(void) -{ - poseSubscriber.shutdown(); - posePublisher.shutdown(); -} - -void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose) -{ - geometry_msgs::PoseStamped stamped; - stamped.header.stamp=ros::Time::now(); - stamped.header.frame_id=frame_id; - stamped.pose.position.x=pose->x; - stamped.pose.position.y=pose->y; - stamped.pose.position.z=0; - stamped.pose.orientation.x=0; - stamped.pose.orientation.y=0; - stamped.pose.orientation.z=sin(pose->theta/2.0); - stamped.pose.orientation.w=cos(pose->theta/2.0); - posePublisher.publish(stamped); -} - -int main(int argc,char* argv[]) -{ - ros::init(argc,argv,"pose2d_stamp"); - ros::NodeHandle node; - - Pose2DStamp pose2DStamp(node); - - ros::Rate loop(100); - ros::Time t0=ros::Time::now(); - while(ros::ok()) - { - ros::spinOnce(); - loop.sleep(); - } - return 0; -}