From e8d5a78d6f51d6ee0e943d89a13dc9cc9af7c2c1 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Tue, 20 Mar 2018 14:38:07 -0300 Subject: [PATCH] twil_controllers: Linearizing controller based on KDL model renamed to cart_linearizing_controller_rne. Implemented new linearizig controller based on identified model Implemented non-smooth backstepping controller (working?) --- twil_controllers/CMakeLists.txt | 5 + twil_controllers/README | 12 +- .../config/nonsmooth_backstep_control.yaml | 11 + .../twil_controllers/cart_linearizing_controller.h | 21 +- .../cart_linearizing_controller_rne.h | 54 +++++ .../nonsmooth_backstep_controller.h | 52 +++++ .../launch/adaptive_nonsmooth_backstep.launch | 21 ++ twil_controllers/launch/cart_linearizing.launch | 9 +- twil_controllers/launch/joint_effort.launch | 9 +- twil_controllers/launch/joint_velocity.launch | 9 +- twil_controllers/launch/nonsmooth_backstep.launch | 16 ++ twil_controllers/mainpage.dox | 14 ++ .../src/cart_linearizing_controller.cpp | 67 ++---- .../src/cart_linearizing_controller_rne.cpp | 162 ++++++++++++++ .../src/nonsmooth_backstep_controller.cpp | 245 +++++++++++++++++++++ twil_controllers/twil_controllers_plugins.xml | 20 ++ 16 files changed, 660 insertions(+), 67 deletions(-) create mode 100644 twil_controllers/config/nonsmooth_backstep_control.yaml create mode 100644 twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h create mode 100644 twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h create mode 100644 twil_controllers/launch/adaptive_nonsmooth_backstep.launch create mode 100644 twil_controllers/launch/nonsmooth_backstep.launch create mode 100644 twil_controllers/mainpage.dox create mode 100644 twil_controllers/src/cart_linearizing_controller_rne.cpp create mode 100644 twil_controllers/src/nonsmooth_backstep_controller.cpp diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt index 97a29cd..2318003 100644 --- a/twil_controllers/CMakeLists.txt +++ b/twil_controllers/CMakeLists.txt @@ -23,8 +23,13 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) #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}) + +find_package(Eigen REQUIRED) +include_directories(${Eigen_INCLUDE_DIRS}) diff --git a/twil_controllers/README b/twil_controllers/README index 326c0d8..acd2807 100644 --- a/twil_controllers/README +++ b/twil_controllers/README @@ -1,3 +1,11 @@ -To publish v: +To publish with joint_effort_controller: -rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.0, 0.04]" +rostopic pub /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" + +To publish with cart_linearizing_controller: + +rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.1, 0.0]" + +To get time and pose: + +rostopic echo -p /gazebo/model_states | awk '{FS=","; printf("%g %g %g %g\n",$1/1e9,$11,$12,2*atan2(sqrt($14^2+$15^2+$16^2),$17))}' diff --git a/twil_controllers/config/nonsmooth_backstep_control.yaml b/twil_controllers/config/nonsmooth_backstep_control.yaml new file mode 100644 index 0000000..d5761f9 --- /dev/null +++ b/twil_controllers/config/nonsmooth_backstep_control.yaml @@ -0,0 +1,11 @@ +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 diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h index bb27196..ca952ba 100644 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller.h +++ b/twil_controllers/include/twil_controllers/cart_linearizing_controller.h @@ -11,10 +11,7 @@ #include #include -#include -#include -#include - +#include namespace twil_controllers { @@ -37,16 +34,14 @@ namespace twil_controllers void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); ros::Subscriber sub_command_; - KDL::ChainIdSolver_RNE *idsolver; - - KDL::JntArray phi; - KDL::JntArray nu; - KDL::JntArray dnu; - KDL::JntArray torque; - std::vector v; - - KDL::Wrenches fext; + Eigen::Vector2d nu; + Eigen::Vector2d torque; + Eigen::Vector2d v; + Eigen::Vector2d u; + Eigen::Matrix2d Ginv; + Eigen::Matrix2d F; + std::vector wheelRadius; double wheelBase; }; diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h new file mode 100644 index 0000000..7aa2911 --- /dev/null +++ b/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h @@ -0,0 +1,54 @@ +#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H +#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace twil_controllers +{ + class CartLinearizingController_RNE: public controller_interface::Controller + { + public: + CartLinearizingController_RNE(void); + ~CartLinearizingController_RNE(void); + + bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); + void starting(const ros::Time& time); + void update(const ros::Time& time); + + private: + ros::NodeHandle node_; + ros::Time last_time_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + + void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); + ros::Subscriber sub_command_; + + KDL::ChainIdSolver_RNE *idsolver; + + KDL::JntArray phi; + KDL::JntArray nu; + KDL::JntArray dnu; + KDL::JntArray torque; + std::vector v; + + KDL::Wrenches fext; + + 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 new file mode 100644 index 0000000..6dbbe98 --- /dev/null +++ b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h @@ -0,0 +1,52 @@ +#ifndef TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H +#define TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H + +#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); + + private: + ros::NodeHandle node_; + ros::Time last_time_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + + ros::Subscriber sub_command_; + ros::Subscriber sub_parameters_; + + Eigen::Matrix2d Ginv; + Eigen::Matrix2d F; + + std::vector wheelRadius; + double wheelBase; + + Eigen::Vector3d xi; + Eigen::Vector3d xiRef; + + Eigen::Vector2d eta; + + void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); + void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); + }; +} +#endif diff --git a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch b/twil_controllers/launch/adaptive_nonsmooth_backstep.launch new file mode 100644 index 0000000..fb8904e --- /dev/null +++ b/twil_controllers/launch/adaptive_nonsmooth_backstep.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/twil_controllers/launch/cart_linearizing.launch b/twil_controllers/launch/cart_linearizing.launch index deccc45..65803f7 100644 --- a/twil_controllers/launch/cart_linearizing.launch +++ b/twil_controllers/launch/cart_linearizing.launch @@ -1,11 +1,16 @@ - + + + + + - + + diff --git a/twil_controllers/launch/joint_effort.launch b/twil_controllers/launch/joint_effort.launch index c1ac051..1138f8a 100644 --- a/twil_controllers/launch/joint_effort.launch +++ b/twil_controllers/launch/joint_effort.launch @@ -1,9 +1,16 @@ - + + + + + + + + diff --git a/twil_controllers/launch/joint_velocity.launch b/twil_controllers/launch/joint_velocity.launch index 4760f53..4c7f96d 100644 --- a/twil_controllers/launch/joint_velocity.launch +++ b/twil_controllers/launch/joint_velocity.launch @@ -1,9 +1,16 @@ - + + + + + + + + diff --git a/twil_controllers/launch/nonsmooth_backstep.launch b/twil_controllers/launch/nonsmooth_backstep.launch new file mode 100644 index 0000000..f8f3a80 --- /dev/null +++ b/twil_controllers/launch/nonsmooth_backstep.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/twil_controllers/mainpage.dox b/twil_controllers/mainpage.dox new file mode 100644 index 0000000..deb448e --- /dev/null +++ b/twil_controllers/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b twil_controllers + + + +--> + + +*/ diff --git a/twil_controllers/src/cart_linearizing_controller.cpp b/twil_controllers/src/cart_linearizing_controller.cpp index 3d8a03c..4080ee5 100644 --- a/twil_controllers/src/cart_linearizing_controller.cpp +++ b/twil_controllers/src/cart_linearizing_controller.cpp @@ -3,12 +3,11 @@ #include #include -#include namespace twil_controllers { CartLinearizingController::CartLinearizingController(void): - phi(0),nu(0),dnu(0),torque(0),v(0),fext(0),wheelRadius(0) + wheelRadius(2) { } @@ -46,9 +45,8 @@ namespace twil_controllers hardware_interface::JointHandle j=robot->getJointHandle((std::string)name_value); joints_.push_back(j); - v.push_back(0); } - sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB, this); + sub_command_ = node_.subscribe("command",1000,&CartLinearizingController::commandCB,this); std::string robot_desc_string; if(!node_.getParam("/robot_description",robot_desc_string)) @@ -64,25 +62,6 @@ namespace twil_controllers return false; } - KDL::Chain chain; - if (!tree.getChain("left_wheel","right_wheel",chain)) - { - ROS_ERROR("Failed to get chain from KDL tree."); - return false; - } - - // Get gravity from gazebo or set values if not simulating - KDL::Vector g; - node_.param("/gazebo/gravity_x",g[1],0.0); - node_.param("/gazebo/gravity_y",g[1],0.0); - node_.param("/gazebo/gravity_z",g[2],-9.8); - - if((idsolver=new KDL::ChainIdSolver_RNE(chain,g)) == NULL) - { - ROS_ERROR("Failed to create ChainIDSolver_RNE."); - 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(); @@ -90,7 +69,7 @@ namespace twil_controllers segmentMapIter=tree.getSegment("right_wheel_support"); KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip(); - wheelRadius.resize(chain.getNrOfJoints()); + wheelRadius.resize(joints_.size()); wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3); // get wheelRadius from URDF (actually from KDL tree) @@ -105,21 +84,22 @@ namespace twil_controllers KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); - // set vectors to the right size - phi.resize(chain.getNrOfJoints()); - nu.resize(chain.getNrOfJoints()); - dnu.resize(chain.getNrOfJoints()); - torque.resize(chain.getNrOfJoints()); + 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; - fext.resize(chain.getNrOfSegments()); - return true; } void CartLinearizingController::starting(const ros::Time& time) { last_time_=time; - for(unsigned int i=0; i < joints_.size();i++) v[i]=0.0; + v.setZero(); } void CartLinearizingController::update(const ros::Time& time) @@ -129,27 +109,18 @@ namespace twil_controllers for(unsigned int i=0;i < joints_.size();i++) { - phi(i)=joints_[i].getPosition(); - nu(i)=joints_[i].getVelocity(); + 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; - dnu(0)=v[0]/wheelRadius[0]-v[1]*wheelBase/2.0/wheelRadius[0]; // left wheel - dnu(1)=v[0]/wheelRadius[1]+v[1]*wheelBase/2.0/wheelRadius[1]; // right wheel + // Compute linearization + Eigen::Vector2d uf; + uf << u[0]*u[1], u[1]*u[1]; + torque=Ginv*(v+F*uf); - for(unsigned int i=0;i < fext.size();i++) fext[i].Zero(); - - for(unsigned int i=0;i < joints_.size();i++) - std::cout << "phi=" << phi(i) << " nu=" << nu(i) - << " dnu=" << dnu(i) << std::endl; - - // Compute linearization. - if(idsolver->CartToJnt(phi,nu,dnu,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed."); - - for(unsigned int i=0;i < joints_.size(); i++) - std::cout << "torque=" << torque(i) << std::endl; - // Apply torques for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i)); } diff --git a/twil_controllers/src/cart_linearizing_controller_rne.cpp b/twil_controllers/src/cart_linearizing_controller_rne.cpp new file mode 100644 index 0000000..3190653 --- /dev/null +++ b/twil_controllers/src/cart_linearizing_controller_rne.cpp @@ -0,0 +1,162 @@ +#include +#include + +#include +#include +#include + +namespace twil_controllers +{ + CartLinearizingController_RNE::CartLinearizingController_RNE(void): + phi(0),nu(0),dnu(0),torque(0),v(0),fext(0),wheelRadius(0) + { + } + + CartLinearizingController_RNE::~CartLinearizingController_RNE(void) + { + sub_command_.shutdown(); + } + + bool CartLinearizingController_RNE::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->getJointHandle((std::string)name_value); + joints_.push_back(j); + v.push_back(0); + } + sub_command_ = node_.subscribe("command",1000,&CartLinearizingController_RNE::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; + } + + KDL::Chain chain; + if (!tree.getChain("left_wheel","right_wheel",chain)) + { + ROS_ERROR("Failed to get chain from KDL tree."); + return false; + } + + // Get gravity from gazebo or set values if not simulating + KDL::Vector g; + node_.param("/gazebo/gravity_x",g[1],0.0); + node_.param("/gazebo/gravity_y",g[1],0.0); + node_.param("/gazebo/gravity_z",g[2],-9.8); + + if((idsolver=new KDL::ChainIdSolver_RNE(chain,g)) == NULL) + { + ROS_ERROR("Failed to create ChainIDSolver_RNE."); + 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(chain.getNrOfJoints()); + 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(); + + // set vectors to the right size + phi.resize(chain.getNrOfJoints()); + nu.resize(chain.getNrOfJoints()); + dnu.resize(chain.getNrOfJoints()); + torque.resize(chain.getNrOfJoints()); + + fext.resize(chain.getNrOfSegments()); + + return true; + } + + 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) + { + ros::Duration dt=time-last_time_; + last_time_=time; + + for(unsigned int i=0;i < joints_.size();i++) + { + phi(i)=joints_[i].getPosition(); + nu(i)=joints_[i].getVelocity(); + } + + + dnu(0)=v[0]/wheelRadius[0]-v[1]*wheelBase/2.0/wheelRadius[0]; // left wheel + dnu(1)=v[0]/wheelRadius[1]+v[1]*wheelBase/2.0/wheelRadius[1]; // right wheel + + + for(unsigned int i=0;i < fext.size();i++) fext[i].Zero(); + +// for(unsigned int i=0;i < joints_.size();i++) +// std::cout << "phi=" << phi(i) << " nu=" << nu(i) +// << " dnu=" << dnu(i) << std::endl; + + // Compute linearization. + if(idsolver->CartToJnt(phi,nu,dnu,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed."); + +// for(unsigned int i=0;i < joints_.size(); i++) +// std::cout << "torque=" << torque(i) << std::endl; + + // Apply torques + for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i)); + } + + void CartLinearizingController_RNE::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command) + { + 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) diff --git a/twil_controllers/src/nonsmooth_backstep_controller.cpp b/twil_controllers/src/nonsmooth_backstep_controller.cpp new file mode 100644 index 0000000..0a37a7f --- /dev/null +++ b/twil_controllers/src/nonsmooth_backstep_controller.cpp @@ -0,0 +1,245 @@ +#include +#include + +#include +#include + +#define sqr(x) (x*x) +#define cub(x) (x*x*x) + +#define NUMERICAL_DETA + +namespace twil_controllers +{ + NonSmoothBackstepController::NonSmoothBackstepController(void): + wheelRadius(2) + { + } + + NonSmoothBackstepController::~NonSmoothBackstepController(void) + { + sub_command_.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->getJointHandle((std::string)name_value); + joints_.push_back(j); + } + 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(); + + 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 NonSmoothBackstepController::starting(const ros::Time& time) + { + last_time_=time; + xi.setZero(); + xiRef.setZero(); + eta.setZero(); + + } + + void NonSmoothBackstepController::update(const ros::Time& time) + { + ros::Duration dt=time-last_time_; + if(dt.toSec() < 0.01) return; + last_time_=time; + + Eigen::Vector2d nu; + for(unsigned int i=0;i < joints_.size();i++) + { + nu[i]=joints_[i].getVelocity(); + } + + Eigen::Vector2d u; + u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0; + u[1]=(nu[1]*wheelRadius[1]-nu[0]*wheelRadius[0])/wheelBase; + + // 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, + 0.0, 1.0; + + xi+=B*u*dt.toSec(); + + // Change of coordinates + Eigen::Matrix3d R; + R << cos(xiRef[2]), sin(xiRef[2]), 0.0, + -sin(xiRef[2]), cos(xiRef[2]), 0.0, + 0.0, 0.0, 1.0; + Eigen::Vector3d xBar=R*(xi-xiRef); + + // Discontinuous transformation + double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); + double psi=atan2(xBar[1],xBar[0]); + double alpha=xBar[2]-psi; + + // Backstep controller + const double GAMMA1=10.0; + const double GAMMA2=1.0; +// const double GAMMA3=1.0; + const double GAMMA4=10.0; + const double GAMMA5=1.0; + const double LAMBDA1=1.0; + const double LAMBDA2=0.1; + const double LAMBDA3=0.1; + const double LAMBDA4=10.0; + const double LAMBDA5=100.0; + +#ifdef NUMERICAL_DETA + Eigen::Vector2d deta=-eta; +#endif + Eigen::Vector2d eta; + + eta[0]=-GAMMA1*e*cos(alpha); + + if(alpha > 1e-6) eta[1]=-GAMMA2*alpha-GAMMA1*sin(alpha)*cos(alpha)+GAMMA1*LAMBDA3*psi*sin(alpha)/LAMBDA2/alpha*cos(alpha); + else eta[1]=GAMMA1*LAMBDA3*psi/LAMBDA2; + + Eigen::Vector2d eb=u-eta; + + Eigen::Vector2d vBar; + if(e > 1e-6) vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha) + +LAMBDA2/LAMBDA4*alpha*sin(alpha)/e + -LAMBDA3/LAMBDA4*psi*sin(alpha)/e; + else vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha); + vBar[1]=-GAMMA5*eb[1]-LAMBDA2/LAMBDA5*alpha; + +#ifndef NUMERICAL_DETA + // Analitic derivates + Eigen::Vector2d deta; + deta[0]=sqr(GAMMA1)*e*cub(cos(alpha))-GAMMA1*GAMMA2*e*alpha*sin(alpha) + +sqr(GAMMA1)*LAMBDA3/LAMBDA2*e*cos(alpha)*sqr(sin(alpha))/alpha*psi; + deta[1]=sqr(GAMMA2)*alpha-2.0*GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi*sin(alpha)/alpha*cos(alpha) + +GAMMA1*GAMMA2*alpha*sqr(cos(alpha))-sqr(GAMMA1)*LAMBDA3/LAMBDA2*cub(cos(alpha))*sin(alpha)/alpha*psi + -GAMMA1*GAMMA2*alpha*sqr(sin(alpha))+sqr(GAMMA1)*LAMBDA3/LAMBDA2*cos(alpha)*cub(sin(alpha))/alpha*psi + -sqr(GAMMA1)*LAMBDA3/LAMBDA2*sqr(cos(alpha))*sqr(sin(alpha))/alpha-GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi + +sqr(GAMMA1*LAMBDA3/LAMBDA2)*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(GAMMA1*LAMBDA3/LAMBDA2)*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi) + +sqr(GAMMA1*LAMBDA3/LAMBDA2*cos(alpha)*sin(alpha)*psi)/cub(alpha); +#else + deta+=eta; + deta/=dt.toSec(); +#endif + + 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++) + { + if(torque[i] > 100.0) torque[i]=100.0; + if(torque[i] < -100.0) torque[i]=-100.0; + joints_[i].setCommand(torque[i]); + } + } + + void NonSmoothBackstepController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command) + { + for(unsigned int i=0;i < command->data.size() && i < 2;i++) xiRef[i]=command->data[i]; + } + + void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command) + { + // 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.5K7 0.5K8 + // 0.5K7 -0.5K8 + + if(command->data[4] < 1e-3) F(0,1)=command->data[0]; + if(command->data[5] < 1e-3) F(1,0)=command->data[1]; + if(command->data[6] < 1e-3) + { + Ginv(0,0)=0.5*command->data[2]; + Ginv(1,0)=0.5*command->data[2]; + } + if(command->data[7] < 1e-3) + { + Ginv(0,1)=0.5*command->data[3]; + Ginv(1,1)=-0.5*command->data[3]; + } + } + +} + +PLUGINLIB_DECLARE_CLASS(twil_controllers,NonSmoothBackstepController,twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase) diff --git a/twil_controllers/twil_controllers_plugins.xml b/twil_controllers/twil_controllers_plugins.xml index 2348f16..15684e6 100644 --- a/twil_controllers/twil_controllers_plugins.xml +++ b/twil_controllers/twil_controllers_plugins.xml @@ -7,5 +7,25 @@ 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. + + -- 2.12.0