Add config and launch files for linearizing and backstep controllers.

diff --git a/tatuira_bringup/config/adaptive_dynamics_linearizing_controller.yaml b/tatuira_bringup/config/adaptive_dynamics_linearizing_controller.yaml
new file mode 120000 (symlink)
index 0000000..0f2e5a6
--- /dev/null
+++ b/tatuira_bringup/config/adaptive_dynamics_linearizing_controller.yaml
@@ -0,0 +1 @@
+dynamics_linearizing_controller.yaml
\ No newline at end of file

diff --git a/tatuira_bringup/config/adaptive_nonsmooth_backstep_controller.yaml b/tatuira_bringup/config/adaptive_nonsmooth_backstep_controller.yaml
new file mode 120000 (symlink)
index 0000000..c276402
--- /dev/null
+++ b/tatuira_bringup/config/adaptive_nonsmooth_backstep_controller.yaml
@@ -0,0 +1 @@
+nonsmooth_backstep_controller.yaml
\ No newline at end of file

diff --git a/tatuira_bringup/config/dynamics_linearizing_controller.yaml b/tatuira_bringup/config/dynamics_linearizing_controller.yaml
new file mode 100644 (file)
index 0000000..7c2c7b1
--- /dev/null
+++ b/tatuira_bringup/config/dynamics_linearizing_controller.yaml
@@ -0,0 +1,22 @@
+# Watch-out: The indentation here is relevant to the semantic!
+
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 100
+  extra_joints:
+    - name: front_bearing_0_joint
+    - name: front_wheel_rim_joint
+
+dynamics_linearizing_controller:
+  type: effort_controllers/DynamicsLinearizingController
+  joints:
+    - left_rim_joint
+    - right_rim_joint
+  F: [0.0, 0.452, -3.212, 0.0]
+  G: [0.0888, 0.0888, -0.2008, 0.2008]
+  wheel_separation: 0.8
+  wheel_radius: [0.12319, 0.12319]
+  odom_frame_id: "odom"
+  base_frame_id: "wheel_center"
+  priority: 99
+  time_step: 0.01

diff --git a/tatuira_bringup/config/nonsmooth_backstep_controller.yaml b/tatuira_bringup/config/nonsmooth_backstep_controller.yaml
new file mode 100644 (file)
index 0000000..427c8bd
--- /dev/null
+++ b/tatuira_bringup/config/nonsmooth_backstep_controller.yaml
@@ -0,0 +1,24 @@
+# Watch-out: The indentation here is relevant to the semantic!
+
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 100
+  extra_joints:
+    - name: front_bearing_0_joint
+    - name: front_wheel_rim_joint
+
+nonsmooth_backstep_controller:
+  type: effort_controllers/NonSmoothBackstepController
+  joints:
+    - left_rim_joint
+    - right_rim_joint
+  F: [0.0, 0.452, -3.212, 0.0]
+  G: [0.0888, 0.0888, -0.2008, 0.2008]
+  lambda: [200.0, 6.0, 6.0, 500.0, 1000.0]
+  gamma: [10.0, 1.0, 10.0, 50.0]
+  wheel_separation: 0.8
+  wheel_radius: [0.12319, 0.12319]
+  odom_frame_id: "odom"
+  base_frame_id: "wheel_center"
+  priority: 99
+  time_step: 0.01

diff --git a/tatuira_bringup/config/nonsmooth_backstep_controller_step.yaml b/tatuira_bringup/config/nonsmooth_backstep_controller_step.yaml
new file mode 100644 (file)
index 0000000..857263c
--- /dev/null
+++ b/tatuira_bringup/config/nonsmooth_backstep_controller_step.yaml
@@ -0,0 +1,24 @@
+# Watch-out: The indentation here is relevant to the semantic!
+
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 100
+  extra_joints:
+    - name: front_bearing_0_joint
+    - name: front_wheel_rim_joint
+
+nonsmooth_backstep_controller:
+  type: effort_controllers/NonSmoothBackstepController
+  joints:
+    - left_rim_joint
+    - right_rim_joint
+  F: [0.0, 0.452, -3.212, 0.0]
+  G: [0.0888, 0.0888, -0.2008, 0.2008]
+  lambda: [200.0, 40.0, 40.0, 500.0, 1000.0]
+  gamma: [10.0, 1.0, 10.0, 50.0]
+  wheel_separation: 0.8
+  wheel_radius: [0.12319, 0.12319]
+  odom_frame_id: "odom"
+  base_frame_id: "wheel_center"
+  priority: 99
+  time_step: 0.01

diff --git a/tatuira_bringup/config/twist_mrac_linearizing_controller.yaml b/tatuira_bringup/config/twist_mrac_linearizing_controller.yaml
new file mode 100644 (file)
index 0000000..f1af1f5
--- /dev/null
+++ b/tatuira_bringup/config/twist_mrac_linearizing_controller.yaml
@@ -0,0 +1,23 @@
+# Watch-out: The indentation here is relevant to the semantic!
+
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 100
+  extra_joints:
+    - name: front_bearing_0_joint
+    - name: front_wheel_rim_joint
+
+twist_mrac_linearizing_controller:
+  type: effort_controllers/TwistMracLinearizingController
+  joints:
+    - left_rim_joint
+    - right_rim_joint
+  F: [0.0, 0.452, -3.212, 0.0]
+  G: [0.0888, 0.0888, -0.2008, 0.2008]
+  Alpha: [10.0, 10.0]
+  wheel_separation: 0.8
+  wheel_radius: [0.12319, 0.12319]
+  odom_frame_id: "odom"
+  base_frame_id: "wheel_center"
+  priority: 99
+  time_step: 0.01

diff --git a/tatuira_bringup/launch/adaptive_dynamics_linearizing_controller.launch b/tatuira_bringup/launch/adaptive_dynamics_linearizing_controller.launch
new file mode 100644 (file)
index 0000000..8fc37a2
--- /dev/null
+++ b/tatuira_bringup/launch/adaptive_dynamics_linearizing_controller.launch
@@ -0,0 +1,9 @@
+<launch>
+  <rosparam file="$(find tatuira_ident)/config/ident.yaml" command="load" />
+  <node name="tatuira_ident" pkg="tatuira_ident" type="tatuira_ident" output="screen">
+       <remap from="dynamic_parameters" to="dynamics_linearizing_controller/dynamic_parameters"/>
+  </node>
+
+  <include file="$(find tatuira_bringup)/launch/dynamics_linearizing_controller.launch" />
+
+</launch>

diff --git a/tatuira_bringup/launch/adaptive_nonsmooth_backstep_controller.launch b/tatuira_bringup/launch/adaptive_nonsmooth_backstep_controller.launch
new file mode 100644 (file)
index 0000000..9ed1bb3
--- /dev/null
+++ b/tatuira_bringup/launch/adaptive_nonsmooth_backstep_controller.launch
@@ -0,0 +1,9 @@
+<launch>
+  <rosparam file="$(find tatuira_ident)/config/ident.yaml" command="load" />
+  <node name="tatuira_ident" pkg="tatuira_ident" type="tatuira_ident" output="screen">
+       <remap from="dynamic_parameters" to="dynamics_linearizing_controller/dynamic_parameters"/>
+  </node>
+
+  <include file="$(find twil_bringup)/launch/nonsmooth_backstep_controller.launch" />
+
+</launch>

diff --git a/tatuira_bringup/launch/dynamics_linearizing_controller.launch b/tatuira_bringup/launch/dynamics_linearizing_controller.launch
new file mode 100644 (file)
index 0000000..2c69505
--- /dev/null
+++ b/tatuira_bringup/launch/dynamics_linearizing_controller.launch
@@ -0,0 +1,4 @@
+<launch>
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="joint_state_controller dynamics_linearizing_controller"/>
+</launch>

diff --git a/tatuira_bringup/launch/gazebo.launch b/tatuira_bringup/launch/gazebo.launch
index 84769e5..f9a7d9f 100644 (file)
--- a/tatuira_bringup/launch/gazebo.launch
+++ b/tatuira_bringup/launch/gazebo.launch
@@ -7,6 +7,8 @@
        <arg name="config" default="$(find tatuira_bringup)/config/$(arg controller).yaml"/>
 
        <arg name="odometry" default="false"/>
+
+       <remap unless="$(arg odometry)" from="$(arg controller)/odom" to="/odom" />
        
        <include file="$(find tatuira_description)/launch/gazebo.launch" >
                <arg name="paused" value="$(arg paused)"/>
@@ -22,11 +24,9 @@
                <rosparam file="$(find tatuira_bringup)/config/odometry_publisher.yaml" command="load" />
 
                <node pkg="arc_odometry" type="odometry_publisher" name="odometry_publisher" />
-
-               <node pkg="tf2_ros" type="static_transform_publisher" name="world_frame_publisher" args="0 0 0 0 0 0 1 map world" />
-               <node pkg="tf2_ros" type="static_transform_publisher" name="odom_frame_publisher" args="0 -0.42294 0 0 0 0.7071068 0.7071068 map odom" />
-               <node pkg="tf2_ros" type="static_transform_publisher" name="wheel_center_publisher" args="0.42294 0 0 0 0 -0.7071068 0.7071068 wheel_center origin_link" />
-
        </group>
 
+       <node pkg="tf2_ros" type="static_transform_publisher" name="world_frame_publisher" args="0 0 0 0 0 0 1 map world" />
+       <node pkg="tf2_ros" type="static_transform_publisher" name="odom_frame_publisher" args="0 -0.42294 0 0 0 0.7071068 0.7071068 map odom" />
+       <node pkg="tf2_ros" type="static_transform_publisher" name="wheel_center_publisher" args="0.42294 0 0 0 0 -0.7071068 0.7071068 wheel_center origin_link" />
 </launch>

diff --git a/tatuira_bringup/launch/gazebo8.launch b/tatuira_bringup/launch/gazebo8.launch
new file mode 100644 (file)
index 0000000..eb0d8a1
--- /dev/null
+++ b/tatuira_bringup/launch/gazebo8.launch
@@ -0,0 +1,26 @@
+<launch>
+  <arg name="paused" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="wam" default="false"/>
+
+  <arg name="controller" default="nonsmooth_backstep_controller"/>
+  <arg name="config" default="$(find tatuira_bringup)/config/$(arg controller).yaml"/>
+
+  <include file="$(find tatuira_bringup)/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="controller" value="$(arg controller)"/>
+    <arg name="config" value="$(arg config)"/>
+  </include>
+
+  <remap from="/command" to="/$(arg controller)/command" />
+
+  <node name="eight_trajectory" pkg="pose2d_trajectories" type="eight_trajectory" respawn="false" output="screen" args="_x:=0.0 _y:=-0.5  _radius:=1.0 _ang_vel:=0.1"/>
+
+  <node name="pose2d_stamp" pkg="pose2d_trajectories" type="pose2d_stamp" respawn="false" output="screen" args="_frame_id:=map" />
+
+  <include file="$(find tatuira_description)/launch/display.launch" />
+
+</launch>

diff --git a/tatuira_bringup/launch/nonsmooth_backstep_controller.launch b/tatuira_bringup/launch/nonsmooth_backstep_controller.launch
new file mode 100644 (file)
index 0000000..6f19272
--- /dev/null
+++ b/tatuira_bringup/launch/nonsmooth_backstep_controller.launch
@@ -0,0 +1,4 @@
+<launch>
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="joint_state_controller nonsmooth_backstep_controller"/>
+</launch>

diff --git a/tatuira_bringup/launch/twist_mrac_linearizing_controller.launch b/tatuira_bringup/launch/twist_mrac_linearizing_controller.launch
new file mode 100644 (file)
index 0000000..97763c3
--- /dev/null
+++ b/tatuira_bringup/launch/twist_mrac_linearizing_controller.launch
@@ -0,0 +1,4 @@
+<launch>
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="joint_state_controller twist_mrac_linearizing_controller"/>
+</launch>

diff --git a/tatuira_description/rviz/display.rviz b/tatuira_description/rviz/display.rviz
index cd0f752..1dd2760 100644 (file)
--- a/tatuira_description/rviz/display.rviz
+++ b/tatuira_description/rviz/display.rviz
@@ -481,6 +481,22 @@ Visualization Manager:
       Topic: /ground_truth
       Unreliable: false
       Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 0; 25; 255
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Reference Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /command_stamped
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

diff --git a/tatuira_ident/src/tatuira_ident.cpp b/tatuira_ident/src/tatuira_ident.cpp
index 88ae85b..c88a741 100644 (file)
--- a/tatuira_ident/src/tatuira_ident.cpp
+++ b/tatuira_ident/src/tatuira_ident.cpp
@@ -80,8 +80,8 @@ class Ident
 
                ros::Subscriber jointStatesSubscriber_;
                ros::Publisher dynParamPublisher_;
-               ros::Publisher leftWheelCommandPublisher_;
                ros::Publisher rightWheelCommandPublisher_;
+               ros::Publisher leftWheelCommandPublisher_;
 
                const int nJoints_;
 
@@ -127,6 +127,8 @@ Ident::Ident(ros::NodeHandle node):
        leftWheelCommandPublisher_=node_.advertise<std_msgs::Float64>("left_wheel_command",1000);
        rightWheelCommandPublisher_=node_.advertise<std_msgs::Float64>("right_wheel_command",1000);
 
+       prbs_[0].seed(0);
+       prbs_[1].seed(1);
        u_.setZero();
        thetaEst1_.setZero();
        thetaEst2_.setZero();
@@ -143,9 +145,9 @@ Ident::~Ident(void)
 void Ident::resetCovariance(void)
 {
        P1_.setIdentity();
-       P1_*=1;
+       P1_*=1e6;
        P2_.setIdentity();
-       P2_*=1;
+       P2_*=1e6;
        iter_=0;
 }
 
@@ -169,8 +171,10 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates)
        u_[0]=(jointStates->velocity[0]*wheelRadius_[0]+jointStates->velocity[1]*wheelRadius_[1])/2.0;
        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)
+       // jointStates->effort[0] is left wheel
+       // jointStates->effort[1] is right wheel
+       torque[0]=jointStates->effort[1];       // torque(k)
+       torque[1]=jointStates->effort[0];       // torque(k)
 
        y+=u_;
        y/=dt.toSec();
@@ -211,12 +215,12 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates)
 
 void Ident::setCommand(void)
 {
-       std_msgs::Float64 leftCommand;
        std_msgs::Float64 rightCommand;
-       leftCommand.data=5*prbs_[0]-2.5;
+       std_msgs::Float64 leftCommand;
        rightCommand.data=5*prbs_[1]-2.5;
-       leftWheelCommandPublisher_.publish(leftCommand);
+       leftCommand.data=5*prbs_[0]-2.5;
        rightWheelCommandPublisher_.publish(rightCommand);
+       leftWheelCommandPublisher_.publish(leftCommand);
 }
 
 int main(int argc,char* argv[])