twil_controllers:

	remove numerical derivates
	add launch file for 8 trajectory
	add python script for step trajectory

diff --git a/twil_controllers/config/nonsmooth_backstep_control.yaml b/twil_controllers/config/nonsmooth_backstep_control.yaml
index afcc9f5..6ff08e6 100644 (file)
--- 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 (file)
--- 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<hardware_interface::EffortJointInterface>
+       class NonSmoothBackstepController: public controller_interface::
+               Controller<hardware_interface::EffortJointInterface>
        {
                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<hardware_interface::JointHandle> joints_;
+               ros::NodeHandle node_;
+               hardware_interface::EffortJointInterface *robot_;
+               std::vector<hardware_interface::JointHandle> joints_;
                        
-                       boost::scoped_ptr<realtime_tools::RealtimePublisher
-                               <twil_controllers::NonSmoothBackstepControllerStatus>
-                               >  status_publisher_ ;
+               boost::scoped_ptr<realtime_tools::RealtimePublisher
+                       <twil_controllers::NonSmoothBackstepControllerStatus>
+                       >  status_publisher_ ;
                                
-                       boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_publisher_;
-                       boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_publisher_;
+               boost::shared_ptr<realtime_tools::RealtimePublisher
+                       <nav_msgs::Odometry> > odom_publisher_;
+               boost::shared_ptr<realtime_tools::RealtimePublisher
+                       <tf::tfMessage> > 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<double> wheelRadius;
-                       double wheelBase;
+               std::vector<double> 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<double> lambda;
-                       std::vector<double> gamma;
+               Eigen::Vector2d phi;
                        
-                       void commandCB(const geometry_msgs::Pose2D::ConstPtr &command);
-                       void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command);
+               std::vector<double> lambda;
+               std::vector<double> 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 (file)
--- a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch
+++ b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch
@@ -4,8 +4,7 @@
        <arg name="use_sim_time" default="true"/>
        <arg name="wam" default="false"/>
 
-  <remap from="/twil/nonsmooth_backstep_controller/command" to="/twil/command" />
-  <remap from="/twil/joint_states" to="/joint_states" />
+       <remap from="/twil/joint_states" to="/joint_states" />
 
        <include file="$(find twil_description)/launch/gazebo.launch" >
                <arg name="paused" value="$(arg paused)"/>
@@ -14,14 +13,6 @@
                <arg name="wam" value="$(arg wam)"/>
        </include>
 
-  <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
-
-  <node name="eight_trajectory" pkg="twil_trajectories" type="eight_trajectory" respawn="false" output="screen" ns="twil" args="_x:=0 _y:=0 _radius:=1.0 _ang_vel:=0.2" />
-
-  [B<node name="pose2d_stamp" pkg="twil_trajectories" type="pose2d_stamp" respawn="false" output="screen" ns="twil" args="_frame_id:=map" />
-
-  <node pkg="tf2_ros" type="static_transform_publisher" name="odom_publisher" args="0 0 0 0 0 0 1 map odom" />
-
-  <include file="$(find twil_description)/launch/display.launch" />
+       <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
 
 </launch>

diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch
new file mode 100644 (file)
index 0000000..6375e93
--- /dev/null
+++ b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch
@@ -0,0 +1,27 @@
+<launch>
+       <arg name="paused" default="true"/>
+       <arg name="headless" default="false"/>
+       <arg name="use_sim_time" default="true"/>
+       <arg name="wam" default="false"/>
+
+  <remap from="/twil/command" to="/twil/nonsmooth_backstep_controller/command" />
+  <remap from="/twil/joint_states" to="/joint_states" />
+
+       <include file="$(find twil_description)/launch/gazebo.launch" >
+               <arg name="paused" value="$(arg paused)"/>
+               <arg name="headless" value="$(arg headless)"/>
+               <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+               <arg name="wam" value="$(arg wam)"/>
+       </include>
+
+  <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
+
+  <node name="eight_trajectory" pkg="twil_trajectories" type="eight_trajectory" respawn="false" output="screen" ns="twil" args="_x:=0.0 _y:=-0.5  _radius:=1.0 _ang_vel:=0.1"/>
+
+  <node name="pose2d_stamp" pkg="twil_trajectories" type="pose2d_stamp" respawn="false" output="screen" ns="twil" args="_frame_id:=map" />
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="odom_publisher" args="0 0 0 0 0 0 1 map odom" />
+
+  <include file="$(find twil_description)/launch/display.launch" />
+
+</launch>

diff --git a/twil_controllers/scripts/pose_step.py b/twil_controllers/scripts/pose_step.py
index f0c772e..1bb1007 100755 (executable)
--- 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 (file)
--- 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];