From: Walter Fetter Lages Date: Mon, 26 Jul 2021 02:48:27 +0000 (-0300) Subject: Add config and launch files for linearizing and backstep controllers. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=c4a5bb0ead98fd87bf24f112ecac466c6d163f8b;p=tatuira.git 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 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 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 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 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 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 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 index 0000000..8fc37a2 --- /dev/null +++ b/tatuira_bringup/launch/adaptive_dynamics_linearizing_controller.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/tatuira_bringup/launch/adaptive_nonsmooth_backstep_controller.launch b/tatuira_bringup/launch/adaptive_nonsmooth_backstep_controller.launch new file mode 100644 index 0000000..9ed1bb3 --- /dev/null +++ b/tatuira_bringup/launch/adaptive_nonsmooth_backstep_controller.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/tatuira_bringup/launch/dynamics_linearizing_controller.launch b/tatuira_bringup/launch/dynamics_linearizing_controller.launch new file mode 100644 index 0000000..2c69505 --- /dev/null +++ b/tatuira_bringup/launch/dynamics_linearizing_controller.launch @@ -0,0 +1,4 @@ + + + diff --git a/tatuira_bringup/launch/gazebo.launch b/tatuira_bringup/launch/gazebo.launch index 84769e5..f9a7d9f 100644 --- a/tatuira_bringup/launch/gazebo.launch +++ b/tatuira_bringup/launch/gazebo.launch @@ -7,6 +7,8 @@ + + @@ -22,11 +24,9 @@ - - - - - + + + diff --git a/tatuira_bringup/launch/gazebo8.launch b/tatuira_bringup/launch/gazebo8.launch new file mode 100644 index 0000000..eb0d8a1 --- /dev/null +++ b/tatuira_bringup/launch/gazebo8.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tatuira_bringup/launch/nonsmooth_backstep_controller.launch b/tatuira_bringup/launch/nonsmooth_backstep_controller.launch new file mode 100644 index 0000000..6f19272 --- /dev/null +++ b/tatuira_bringup/launch/nonsmooth_backstep_controller.launch @@ -0,0 +1,4 @@ + + + diff --git a/tatuira_bringup/launch/twist_mrac_linearizing_controller.launch b/tatuira_bringup/launch/twist_mrac_linearizing_controller.launch new file mode 100644 index 0000000..97763c3 --- /dev/null +++ b/tatuira_bringup/launch/twist_mrac_linearizing_controller.launch @@ -0,0 +1,4 @@ + + + diff --git a/tatuira_description/rviz/display.rviz b/tatuira_description/rviz/display.rviz index cd0f752..1dd2760 100644 --- 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 --- 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("left_wheel_command",1000); rightWheelCommandPublisher_=node_.advertise("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[])