[Personalrobots-commit] SF.net SVN: personalrobots:[4375] pkg/trunk

[<hsu...@us...>]

Revision: 4375
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4375&view=rev
Author:   hsujohnhsu
Date:     2008-09-17 02:25:05 +0000 (Wed, 17 Sep 2008)

Log Message:
-----------
update controllers to use shortest_angular_distance for steering angle.
update gains in controllers.xml for slightly improved steering.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml

Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp	2008-09-17 02:13:25 UTC (rev 4374)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp	2008-09-17 02:25:05 UTC (rev 4375)
@@ -298,11 +298,11 @@
   {
     result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
     steer_angle_desired = atan2(result.y,result.x);
-//     steer_angle_desired =  modNPiBy2(steer_angle_desired);//Clean steer Angle
+    //     steer_angle_desired =  modNPiBy2(steer_angle_desired);//Clean steer Angle
 
-    error_steer = steer_angle_actual_[i] - steer_angle_desired;
+    error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
     steer_velocity_desired_[i] = -kp_speed_*error_steer;
-//    std::cout << "setting steering velocity " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << "error_steer" << error_steer << std::endl;
+    //    std::cout << "setting steering velocity " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << "error_steer" << error_steer << std::endl;
     base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
   }
 }
@@ -327,13 +327,13 @@
     steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
     wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
 
-//    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
+    //    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
 
     wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-//    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
+    //    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
     wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
     wheel_speed_cmd = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
-//    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
+    //    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
     base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd);
   }
 

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-09-17 02:13:25 UTC (rev 4374)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml	2008-09-17 02:25:05 UTC (rev 4375)
@@ -8,7 +8,7 @@
     </map>
     <controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
         <joint name="caster_front_left_joint" >
-          <pid p="1" d="0" i="0" iClamp="0" />
+          <pid p="3" d="0" i="0.001" iClamp="0.01" />
         </joint>
     </controller>
     <controller name="wheel_front_left_l_controller" topic="wheel_front_left_l_controller" type="JointVelocityControllerNode">
@@ -23,7 +23,7 @@
     </controller>
     <controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
         <joint name="caster_front_right_joint" >
-          <pid p="1" d="0" i="0" iClamp="0" />
+          <pid p="3" d="0" i="0.001" iClamp="0.01" />
         </joint>
     </controller>
     <controller name="wheel_front_right_l_controller" topic="wheel_front_right_l_controller" type="JointVelocityControllerNode">
@@ -38,7 +38,7 @@
     </controller>
     <controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
         <joint name="caster_rear_left_joint" >
-          <pid p="1" d="0" i="0" iClamp="0" />
+          <pid p="3" d="0" i="0.001" iClamp="0.01" />
         </joint>
     </controller>
     <controller name="wheel_rear_left_l_controller" topic="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
@@ -53,7 +53,7 @@
     </controller>
     <controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
         <joint name="caster_rear_right_joint" >
-          <pid p="1" d="0" i="0" iClamp="0" />
+          <pid p="3" d="0" i="0.001" iClamp="0.01" />
         </joint>
     </controller>
     <controller name="wheel_rear_right_l_controller" topic="wheel_rear_right_l_controller" type="JointVelocityControllerNode">


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.