From 20fa0d618c6b35f63b9e81f3ef4dcd5db31c512f Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 21 Mar 2018 17:44:53 -0300 Subject: [PATCH] twil_controllers: remove numerical derivates add launch file for 8 trajectory add python script for step trajectory --- .../config/nonsmooth_backstep_control.yaml | 2 +- .../nonsmooth_backstep_controller.h | 62 +++++----- .../{gazebo.launch => gazebo_joint_effort.launch} | 0 .../launch/gazebo_nonsmooth_backstep.launch | 13 +-- .../launch/gazebo_nonsmooth_backstep8.launch | 27 +++++ twil_controllers/scripts/pose_step.py | 5 +- .../src/nonsmooth_backstep_controller.cpp | 130 +++++++++++---------- 7 files changed, 137 insertions(+), 102 deletions(-) rename twil_controllers/launch/{gazebo.launch => gazebo_joint_effort.launch} (100%) create mode 100644 twil_controllers/launch/gazebo_nonsmooth_backstep8.launch diff --git a/twil_controllers/config/nonsmooth_backstep_control.yaml b/twil_controllers/config/nonsmooth_backstep_control.yaml index afcc9f5..6ff08e6 100644 --- a/twil_controllers/config/nonsmooth_backstep_control.yaml +++ b/twil_controllers/config/nonsmooth_backstep_control.yaml @@ -1,4 +1,4 @@ -# Watch-out: The indenting here is relevant to the semantic! +# Watch-out: The indentation here is relevant to the semantic! twil: diff --git a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h index c00b0f2..f37692e 100644 --- a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h +++ b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h @@ -23,49 +23,55 @@ namespace twil_controllers { - class NonSmoothBackstepController: public controller_interface::Controller + class NonSmoothBackstepController: public controller_interface:: + Controller { public: - NonSmoothBackstepController(void); - ~NonSmoothBackstepController(void); + 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); + 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_; + ros::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; - boost::scoped_ptr - > status_publisher_ ; + boost::scoped_ptr + > status_publisher_ ; - boost::shared_ptr > odom_publisher_; - boost::shared_ptr > tf_odom_publisher_; + boost::shared_ptr > odom_publisher_; + boost::shared_ptr > tf_odom_publisher_; - ros::Subscriber sub_command_; - ros::Subscriber sub_parameters_; + ros::Subscriber sub_command_; + ros::Subscriber sub_parameters_; - Eigen::Matrix2d Ginv; - Eigen::Matrix2d F; + Eigen::Matrix2d Ginv; + Eigen::Matrix2d F; - std::vector wheelRadius; - double wheelBase; + std::vector wheelRadius; + double wheelBase; - Eigen::Vector3d x; - Eigen::Vector3d xRef; + Eigen::Vector3d x; + Eigen::Vector3d xRef; - Eigen::Vector2d eta; + Eigen::Vector2d eta; - ros::Time lastSamplingTime; + ros::Time lastSamplingTime; - std::vector lambda; - std::vector gamma; + Eigen::Vector2d phi; - void commandCB(const geometry_msgs::Pose2D::ConstPtr &command); - void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); + 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/gazebo.launch b/twil_controllers/launch/gazebo_joint_effort.launch similarity index 100% rename from twil_controllers/launch/gazebo.launch rename to twil_controllers/launch/gazebo_joint_effort.launch diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch index 45e7518..16002aa 100644 --- a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch +++ b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch @@ -4,8 +4,7 @@ - - + @@ -14,14 +13,6 @@ - - - - - [B - - - - + diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch new file mode 100644 index 0000000..6375e93 --- /dev/null +++ b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_controllers/scripts/pose_step.py b/twil_controllers/scripts/pose_step.py index f0c772e..1bb1007 100755 --- a/twil_controllers/scripts/pose_step.py +++ b/twil_controllers/scripts/pose_step.py @@ -15,12 +15,15 @@ def step(): pose.y=float(sys.argv[2]) pose.theta=float(sys.argv[3]) - pub=rospy.Publisher('/twil/nonsmooth_backstep_controller/command', Pose2D, queue_size=100) + pub=rospy.Publisher('/twil/nonsmooth_backstep_controller/command', Pose2D, queue_size=1) rospy.init_node('pose_step',anonymous=True) pub.publish(pose) time.sleep(1) + + pub.publish(pose) + time.sleep(1) if __name__ == '__main__': try: diff --git a/twil_controllers/src/nonsmooth_backstep_controller.cpp b/twil_controllers/src/nonsmooth_backstep_controller.cpp index 5abdcd7..b2acae8 100644 --- a/twil_controllers/src/nonsmooth_backstep_controller.cpp +++ b/twil_controllers/src/nonsmooth_backstep_controller.cpp @@ -9,10 +9,6 @@ #define sqr(x) (x*x) #define cub(x) (x*x*x) -// Are numeric derivates of eta too noisy? -// It is not simple to compute it analitically... -#define NUMERICAL_DETA - namespace twil_controllers { NonSmoothBackstepController::NonSmoothBackstepController(void): @@ -139,17 +135,17 @@ namespace twil_controllers KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); - gamma[0]=1; - gamma[1]=1; - gamma[2]=1; - gamma[3]=1; + gamma[0]=10.0; + gamma[1]=1.0; + gamma[2]=10.0; + gamma[3]=10.0; node_.param("gamma",gamma,gamma); - lambda[0]=1; - lambda[1]=1; - lambda[2]=1; - lambda[3]=1; - lambda[4]=1; + 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; @@ -171,6 +167,10 @@ namespace twil_controllers 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, @@ -181,31 +181,34 @@ namespace twil_controllers if(fabs(dt-0.01) > 0.001/2) return; lastSamplingTime=time; - Eigen::Vector2d w; + // 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++) { - w[i]=joints_[i].getVelocity(); + phi[i]=joints_[i].getPosition(); } + deltaPhi+=phi; - // w[0] is the left wheel velocity - // w[1] is the right wheel velocity - Eigen::Vector2d u; - u[0]=(w[0]*wheelRadius[0]+w[1]*wheelRadius[1])/2.0; - u[1]=(w[1]*wheelRadius[1]-w[0]*wheelRadius[0])/wheelBase; - // Estimate pose by odometry - Eigen::MatrixXd B(3,2); - double deltaTheta2=u[1]*dt/2.0; - double deltaS=(fabs(deltaTheta2) > 1e-10)? sin(deltaTheta2)/deltaTheta2:1.0; + 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]/2)/(deltaU[1]/2):deltaU[0]; - B << cos(x[2]+deltaS*deltaTheta2), 0.0, - sin(x[2]+deltaS*deltaTheta2), 0.0, - 0.0, 1.0; + Eigen::Vector3d deltaX; + deltaX << deltaS*cos(x[2]+deltaU[1]/2.0), + deltaS*sin(x[2]+deltaU[1]/2.0), + deltaU[1]; - Eigen::Vector3d dx=B*u; - - x+=dx*dt; + x+=deltaX; + Eigen::Vector2d u=deltaU/dt; + // Change of coordinates Eigen::Matrix3d R; R << cos(xRef[2]), sin(xRef[2]), 0.0, @@ -218,12 +221,13 @@ namespace twil_controllers double psi=atan2(xBar[1],xBar[0]); double alpha=xBar[2]-psi; -#ifdef NUMERICAL_DETA - Eigen::Vector2d deta=-eta; -#endif + // 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); + 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; @@ -235,29 +239,32 @@ namespace twil_controllers else vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha); vBar[1]=-gamma[3]*eb[1]-lambda[1]/lambda[4]*alpha; -#ifdef NUMERICAL_DETA - // Numerical derivates - deta+=eta; - deta/=dt; -#else - // Analitic derivates Eigen::Vector2d deta; - deta[0]=sqr(gamma[0])*e*cub(cos(alpha))-gamma[0]*gamma[1]*e*alpha*sin(alpha) - +sqr(gamma[0])*lambda[2]/lambda[1]*e*cos(alpha)*sqr(sin(alpha))/alpha*psi; - deta[1]=sqr(gamma[1])*alpha-2.0*gamma[0]*gamma[1]*lambda[2]/lambda[1]*psi*sin(alpha)/alpha*cos(alpha) - +gamma[0]*gamma[1]*alpha*sqr(cos(alpha))-sqr(gamma[0])*lambda[2]/lambda[1]*cub(cos(alpha))*sin(alpha)/alpha*psi - -gamma[0]*gamma[1]*alpha*sqr(sin(alpha))+sqr(gamma[0])*lambda[2]/lambda[1]*cos(alpha)*cub(sin(alpha))/alpha*psi - -sqr(gamma[0])*lambda[2]/lambda[1]*sqr(cos(alpha))*sqr(sin(alpha))/alpha-gamma[0]*gamma[1]*lambda[2]/lambda[1]*psi - +sqr(gamma[0]*lambda[2]/lambda[1])*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(gamma[0]*lambda[2]/lambda[1])*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi) - +sqr(gamma[0]*lambda[2]/lambda[1]*cos(alpha)*sin(alpha)*psi)/cub(alpha); -#endif + + 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]); - + // Linearization + Eigen::Vector2d uf(u[0]*u[1],sqr(u[1])); Eigen::Vector2d torque=Ginv*(v-F*uf); // Apply torques @@ -277,10 +284,10 @@ namespace twil_controllers 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=dx[0]; - status_publisher_->msg_.process_value_dot.y=dx[1]; - status_publisher_->msg_.process_value_dot.theta=dx[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]; @@ -331,16 +338,17 @@ namespace twil_controllers 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=dx[0]; - odom_publisher_->msg_.twist.twist.linear.y=dx[1]; - odom_publisher_->msg_.twist.twist.angular.z=dx[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]; + 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]; -- 2.12.0