Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12062] pkg/trunk
|
From: <vij...@us...> - 2009-03-03 22:40:46
|
Revision: 12062
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12062&view=rev
Author: vijaypradeep
Date: 2009-03-03 22:40:37 +0000 (Tue, 03 Mar 2009)
Log Message:
-----------
Working on updating laser_traj_controller to use the new filter API/xml
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-03-03 22:39:59 UTC (rev 12061)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-03-03 22:40:37 UTC (rev 12062)
@@ -84,7 +84,7 @@
private:
- void initDErrorFilter(double f) ; // Initializes d_error_filter using a smoothing factor
+ //void initDErrorFilter(double f) ; // Initializes d_error_filter using a smoothing factor
mechanism::RobotState *robot_ ;
mechanism::JointState *joint_state_ ; // Need this to check the calibrated flag on the joint
@@ -111,7 +111,7 @@
// Control loop state
control_toolbox::Pid pid_controller_ ; // Position PID Controller
- filters::TransferFunctionFilter<double >* d_error_filter ; // Filter on derivative term of error
+ filters::TransferFunctionFilter<double> d_error_filter ; // Filter on derivative term of error
double last_time_ ; // The previous time at which the control loop was run
double last_error_ ; // Error for the previous time at which the control loop was run
double tracking_offset_ ; // Position cmd generated by the track_link code
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-03-03 22:39:59 UTC (rev 12061)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-03-03 22:40:37 UTC (rev 12062)
@@ -47,13 +47,11 @@
{
tracking_offset_ = 0 ;
track_link_enabled_ = false ;
- d_error_filter = NULL ;
}
LaserScannerTrajController::~LaserScannerTrajController()
{
- if (d_error_filter != NULL)
- delete d_error_filter ;
+
}
bool LaserScannerTrajController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
@@ -109,21 +107,15 @@
last_time_ = robot->hw_->current_time_ ;
last_error_ = 0.0 ;
- // ***** Derivate Error Filter Element *****
- TiXmlElement *filter_elem = config->FirstChildElement("d_error_filter");
+ // ***** Derivative Error Filter Element *****
+ TiXmlElement *filter_elem = config->FirstChildElement("filter");
if(!filter_elem)
{
- ROS_ERROR("%s:: d_error_filter element not defined inside controller", name_.c_str()) ;
+ ROS_ERROR("%s:: filter element not defined inside controller", name_.c_str()) ;
return false ;
}
- double smoothing_factor ;
- if(filter_elem->QueryDoubleAttribute("smoothing_factor", & smoothing_factor)!=TIXML_SUCCESS)
- {
- ROS_ERROR("%s:: Error reading \"smoothing_factor\" element in d_error_filter element", name_.c_str()) ;
- return false ;
- }
- initDErrorFilter(smoothing_factor) ;
+ d_error_filter.FilterBase<double>::configure((unsigned int)1, filter_elem) ;
// ***** Max Rate and Acceleration Elements *****
TiXmlElement *max_rate_elem = config->FirstChildElement("max_rate") ;
@@ -141,21 +133,6 @@
return true ;
}
-void LaserScannerTrajController::initDErrorFilter(double f)
-{
- vector<double> a ;
- vector<double> b ;
- a.resize(2) ;
- b.resize(1) ;
- a[0] = 1.0 ;
- a[1] = - (1.0 - f) ;
- b[0] = 1.0 ;
-
- if(d_error_filter == NULL)
- delete d_error_filter ;
- d_error_filter = new TransferFunctionFilter<double>(b,a,1) ;
-}
-
void LaserScannerTrajController::update()
{
if (!joint_state_->calibrated_)
@@ -215,16 +192,13 @@
error) ;
double dt = time - last_time_ ;
double d_error = (error-last_error_)/dt ;
- vector<double> filtered_d_error ;
- filtered_d_error.resize(1) ;
+ double filtered_d_error ;
- vector<double> d_error_vec(1,d_error) ;
+ d_error_filter.update(d_error, filtered_d_error) ;
- d_error_filter->update(&d_error_vec, &filtered_d_error) ;
-
// Add filtering step
// Update pid with d_error added
- joint_state_->commanded_effort_ = pid_controller_.updatePid(error, filtered_d_error[0], dt) ;
+ joint_state_->commanded_effort_ = pid_controller_.updatePid(error, filtered_d_error, dt) ;
last_time_ = time ;
last_error_ = error ;
}
@@ -417,6 +391,8 @@
prev_profile_time_ = 0.0 ;
+ ROS_INFO("Successfully spawned %s", service_prefix_.c_str()) ;
+
return true ;
}
Modified: pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml 2009-03-03 22:39:59 UTC (rev 12061)
+++ pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml 2009-03-03 22:40:37 UTC (rev 12062)
@@ -2,7 +2,9 @@
<controller name="laser_tilt_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
<max_acc value="1.0" />
<max_rate value="100.0" />
- <d_error_filter smoothing_factor=".05" />
+ <filter name="d_error_filter" type="TransferFunctionFilter">
+ <params a="1.0 -.1" b=".9" />
+ </filter>
<joint name="laser_tilt_mount_joint">
<pid p="8" i=".1" d="0.2" iClamp="0.5" />
</joint>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
