test_joint_position_controller.cpp
Go to the documentation of this file.
00001 
00027 #include "sr_mechanism_controllers/test/test_controllers.hpp"
00028 
00029 #include "sr_mechanism_controllers/srh_joint_position_controller.hpp"
00030 
00031 #include <gtest/gtest.h>
00032 
00033 using namespace controller;
00034 
00035 class TestJointPositionController : public TestControllers
00036 {
00037 public:
00038   boost::shared_ptr<control_toolbox::Pid> pid;
00039 
00040   TestJointPositionController(boost::shared_ptr<control_toolbox::Pid> pid) :
00041     TestControllers()
00042   {
00043     this->pid = pid;
00044     controller = boost::shared_ptr<SrhJointPositionController>( new SrhJointPositionController() );
00045 
00046     init();
00047   }
00048 
00049   ~TestJointPositionController()
00050   {}
00051 
00052   void init_controller()
00053   {
00054     controller::SrController* control_tmp = controller.get();
00055     controller::SrhJointPositionController* sr_control_tmp = dynamic_cast< controller::SrhJointPositionController* >( control_tmp );
00056     sr_control_tmp->init(robot_state.get(), "FFJ3", pid);
00057   }
00058 
00059   double compute_output(double input, double current_position)
00060   {
00061     hw->current_time_ = ros::Time::now();
00062     joint_state->position_ = current_position;
00063     controller->setCommand( input );
00064 
00065     controller->update();
00066 
00067     return joint_state->commanded_effort_;
00068   }
00069 };
00070 
00071 TEST(SrhJointPositionController, TestPID)
00072 {
00073   //TESTING A PURE P CONTROLLER
00074   boost::shared_ptr<control_toolbox::Pid> pid;
00075   pid = boost::shared_ptr<control_toolbox::Pid>( new control_toolbox::Pid(1.0, 0.0, 0.0, 0.0, 0.0) );
00076 
00077   boost::shared_ptr<TestJointPositionController> test_jpc;
00078   test_jpc = boost::shared_ptr<TestJointPositionController>( new TestJointPositionController( pid ) );
00079 
00080   const unsigned int nb_values = 7;
00081 
00082   bool with_friction_compensation = false;
00083   if( ros::param::has("with_friction_compensation") )
00084   {
00085     int wfc;
00086     ros::param::get("with_friction_compensation", wfc);
00087     if( wfc == 1)
00088       with_friction_compensation = true;
00089     else
00090       with_friction_compensation = false;
00091   }
00092 
00093   const double values[nb_values] = {-123.123, -1.0, -0.5, 0.0, 0.5, 1.0, 456.456};
00094   const double expected_values_no_fc[nb_values] = {-123.123, -1.0, -0.5, 0.0, 0.5, 1.0, 456.456};
00095   const double expected_values_with_fc[nb_values] = {-323.123, -1.0, -0.5, 0.0, 0.5, 1.0, 656.456};
00096 
00097   ros::Duration pause(0.01);
00098   double ctrl_output = 0.0;
00099   for(unsigned int i = 0; i < nb_values; ++i)
00100   {
00101     ROS_INFO_STREAM("Sending demand: "<<values[i]);
00102     pause.sleep();
00103     ctrl_output = test_jpc->compute_output( values[i], 0.0 );
00104     if(with_friction_compensation)
00105     {
00106       ROS_INFO_STREAM("Expected value: "<< expected_values_with_fc[i] << " Computed value: " <<ctrl_output);
00107       EXPECT_EQ(ctrl_output, expected_values_with_fc[i]);
00108     }
00109     else
00110     {
00111       ROS_INFO_STREAM("Expected value: "<< expected_values_no_fc[i] << " Computed value: " <<ctrl_output);
00112       EXPECT_EQ(ctrl_output, expected_values_no_fc[i]);
00113     }
00114   }
00115 
00116   //Test the position deadband as well:
00117   double target = 0.1;
00118   const double pos[5] = {0.0, 0.11, 0.099, 0.116, 0.115};
00119   const double expected_values[5] = {0.1, 0.0, 0.0, -0.016, 0.0};
00120   for(unsigned int i = 0; i < 5; ++i)
00121   {
00122     pause.sleep();
00123     ctrl_output = test_jpc->compute_output( target, pos[i] );
00124     ROS_INFO_STREAM("Expected value: "<< expected_values[i] << " Computed value: " <<ctrl_output);
00125     EXPECT_EQ(ctrl_output, expected_values[i]);
00126   }
00127 
00128   //TESTING A PURE I CONTROLLER
00129   pid->reset();
00130   pid->setGains(0.0, 1.0, 0.0, 2.0, -2.0);
00131 
00132   ros::Duration one_sec_pause(1.0);
00133   ros::Duration half_sec_pause(0.5);
00134   //ros::Duration pause(0.01);
00135 
00136   const double expected_values_one_sec[nb_values] = {-2.0, -1.0, -0.5, 0.0, 0.5, 1.0, 2.0};
00137   const double expected_values_half_sec[nb_values] = {-2.0, -0.5, -0.25, 0.0, 0.25, 0.5, 2.0};
00138 
00139   for(unsigned int i = 0; i < nb_values; ++i)
00140   {
00141     ROS_INFO_STREAM("Sending demand: "<<values[i]);
00142     pid->reset();
00143     pause.sleep();
00144     ctrl_output = test_jpc->compute_output( 0.0, 0.0 );
00145     one_sec_pause.sleep();
00146     ctrl_output = test_jpc->compute_output( values[i], 0.0 );
00147 
00148     ROS_INFO_STREAM("Expected value: "<< expected_values_one_sec[i] << " Computed value: " <<ctrl_output);
00149     EXPECT_TRUE( fabs(ctrl_output - expected_values_one_sec[i]) < 0.001 );
00150   }
00151   for(unsigned int i = 0; i < nb_values; ++i)
00152   {
00153     ROS_INFO_STREAM("Sending demand: "<<values[i]);
00154     pid->reset();
00155     pause.sleep();
00156     ctrl_output = test_jpc->compute_output( 0.0, 0.0 );
00157     half_sec_pause.sleep();
00158     ctrl_output = test_jpc->compute_output( values[i], 0.0 );
00159 
00160     ROS_INFO_STREAM("Expected value: "<< expected_values_half_sec[i] << " Computed value: " <<ctrl_output);
00161     EXPECT_TRUE( fabs(ctrl_output - expected_values_half_sec[i]) < 0.001 );
00162   }
00163 }
00164 
00165 
00167 //     MAIN       //
00169 
00170 int main(int argc, char **argv)
00171 {
00172   ros::init(argc, argv, "calibration_test");
00173 
00174   testing::InitGoogleTest(&argc, argv);
00175   return RUN_ALL_TESTS();
00176 }
00177 
00178 /* For the emacs weenies in the crowd.
00179    Local Variables:
00180    c-basic-offset: 2
00181    End:
00182 */


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:54:39