Merge lp:~shadowrobot/sr-ros-interface-ethercat/Feature_update_controller_params into lp:sr-ros-interface-ethercat

Proposed by Toni Oliver on 2012-11-08
Status: Merged
Approved by: Toni Oliver on 2012-11-09
Approved revision: 564
Merged at revision: 564
Proposed branch: lp:~shadowrobot/sr-ros-interface-ethercat/Feature_update_controller_params
Merge into: lp:sr-ros-interface-ethercat
Diff against target: 138 lines (+87/-3)
3 files modified
sr_robot_lib/include/sr_robot_lib/sr_hand_lib.hpp (+25/-0)
sr_robot_lib/src/sr_hand_lib.cpp (+61/-2)
sr_robot_lib/src/sr_robot_lib.cpp (+1/-1)
To merge this branch: bzr merge lp:~shadowrobot/sr-ros-interface-ethercat/Feature_update_controller_params
Reviewer Review Type Date Requested Status
markpitchless 2012-11-08 Pending
Review via email: mp+133457@code.launchpad.net

Commit message

Update params in param server when changing controller parameters.

Description of the change

Update params in param server when changing controller parameters.

To post a comment you must log in.

Preview Diff

[H/L] Next/Prev Comment, [J/K] Next/Prev File, [N/P] Next/Prev Hunk
1=== modified file 'sr_robot_lib/include/sr_robot_lib/sr_hand_lib.hpp'
2--- sr_robot_lib/include/sr_robot_lib/sr_hand_lib.hpp 2012-08-20 09:38:26 +0000
3+++ sr_robot_lib/include/sr_robot_lib/sr_hand_lib.hpp 2012-11-08 11:55:23 +0000
4@@ -100,6 +100,31 @@
5 std::vector<shadow_joints::JointToSensor> joint_to_sensors,
6 std::vector<sr_actuator::SrActuator*> actuators);
7
8+ /**
9+ * Updates the parameter values for the force control in the Parameter Server
10+ *
11+ * @param joint_name The name of the joint.
12+ * @param max_pwm The max pwm the motor will apply
13+ * @param sg_left Strain gauge left
14+ * @param sg_right Strain gauge right
15+ * @param f The feedforward term (directly adds f*error to the output of the PID)
16+ * @param p The p value.
17+ * @param i the i value.
18+ * @param d the d value.
19+ * @param imax the imax value.
20+ * @param deadband the deadband on the force.
21+ * @param sign can be 0 or 1 depending on the way the motor is plugged in.
22+ */
23+ void update_force_control_in_param_server(std::string joint_name, int max_pwm, int sg_left, int sg_right, int f, int p,
24+ int i, int d, int imax, int deadband, int sign);
25+
26+ /**
27+ * Finds the joint name for a certain motor index
28+ *
29+ * @param motor_index The integer motor index
30+ */
31+ std::string find_joint_name(int motor_index);
32+
33 private:
34 ros::NodeHandle nodehandle_;
35
36
37=== modified file 'sr_robot_lib/src/sr_hand_lib.cpp'
38--- sr_robot_lib/src/sr_hand_lib.cpp 2012-08-20 09:38:26 +0000
39+++ sr_robot_lib/src/sr_hand_lib.cpp 2012-11-08 11:55:23 +0000
40@@ -283,10 +283,10 @@
41 full_param << "/" << act_name << "/pid/max_pwm";
42 nodehandle_.param<int>(full_param.str(), max_pwm, 0);
43 full_param.str("");
44- full_param << "/" << act_name << "/pid/sg_left";
45+ full_param << "/" << act_name << "/pid/sgleftref";
46 nodehandle_.param<int>(full_param.str(), sg_left, 0);
47 full_param.str("");
48- full_param << "/" << act_name << "/pid/sg_right";
49+ full_param << "/" << act_name << "/pid/sgrightref";
50 nodehandle_.param<int>(full_param.str(), sg_right, 0);
51 full_param.str("");
52 full_param << "/" << act_name << "/pid/deadband";
53@@ -432,6 +432,10 @@
54 generate_force_control_config( motor_index, request.maxpwm, request.sgleftref,
55 request.sgrightref, request.f, request.p, request.i,
56 request.d, request.imax, request.deadband, request.sign );
57+
58+ update_force_control_in_param_server( find_joint_name(motor_index), request.maxpwm, request.sgleftref,
59+ request.sgrightref, request.f, request.p, request.i,
60+ request.d, request.imax, request.deadband, request.sign);
61 response.configured = true;
62
63 //Reinitialize motors information
64@@ -440,6 +444,61 @@
65 return true;
66 }
67
68+ std::string SrHandLib::find_joint_name(int motor_index)
69+ {
70+ for( boost::ptr_vector<shadow_joints::Joint>::iterator joint = joints_vector.begin();
71+ joint != joints_vector.end(); ++joint )
72+ {
73+ if( !boost::is_null(joint) ) // check for validity
74+ {
75+ if(joint->motor->motor_id == motor_index)
76+ return joint->joint_name;
77+ }
78+ }
79+ ROS_ERROR("Could not find joint name for motor index: %d", motor_index);
80+ return "";
81+ }
82+
83+ void SrHandLib::update_force_control_in_param_server(std::string joint_name, int max_pwm, int sg_left, int sg_right, int f, int p,
84+ int i, int d, int imax, int deadband, int sign)
85+ {
86+ if(joint_name != "")
87+ {
88+ std::stringstream full_param;
89+ std::string act_name = boost::to_lower_copy(joint_name);
90+
91+ full_param << "/" << act_name << "/pid/f";
92+ nodehandle_.setParam(full_param.str(), f);
93+ full_param.str("");
94+ full_param << "/" << act_name << "/pid/p";
95+ nodehandle_.setParam(full_param.str(), p);
96+ full_param.str("");
97+ full_param << "/" << act_name << "/pid/i";
98+ nodehandle_.setParam(full_param.str(), i);
99+ full_param.str("");
100+ full_param << "/" << act_name << "/pid/d";
101+ nodehandle_.setParam(full_param.str(), d);
102+ full_param.str("");
103+ full_param << "/" << act_name << "/pid/imax";
104+ nodehandle_.setParam(full_param.str(), imax);
105+ full_param.str("");
106+ full_param << "/" << act_name << "/pid/max_pwm";
107+ nodehandle_.setParam(full_param.str(), max_pwm);
108+ full_param.str("");
109+ full_param << "/" << act_name << "/pid/sgleftref";
110+ nodehandle_.setParam(full_param.str(), sg_left);
111+ full_param.str("");
112+ full_param << "/" << act_name << "/pid/sgrightref";
113+ nodehandle_.setParam(full_param.str(), sg_right);
114+ full_param.str("");
115+ full_param << "/" << act_name << "/pid/deadband";
116+ nodehandle_.setParam(full_param.str(), deadband);
117+ full_param.str("");
118+ full_param << "/" << act_name << "/pid/sign";
119+ nodehandle_.setParam(full_param.str(), sign);
120+ }
121+ }
122+
123
124 std::vector<shadow_joints::JointToSensor> SrHandLib::read_joint_to_sensor_mapping()
125 {
126
127=== modified file 'sr_robot_lib/src/sr_robot_lib.cpp'
128--- sr_robot_lib/src/sr_robot_lib.cpp 2012-10-29 14:18:42 +0000
129+++ sr_robot_lib/src/sr_robot_lib.cpp 2012-11-08 11:55:23 +0000
130@@ -934,7 +934,7 @@
131 int i, int d, int imax, int deadband, int sign)
132 {
133 ROS_INFO_STREAM(
134- "Setting new pid values for motor" << motor_index << ": max_pwm="<< max_pwm <<" sg_left=" << sg_left << " sg_right=" << sg_right << " f=" << f << " p=" << p << " i=" << i << " d="<< d << " imax=" << imax << " deadband="<< deadband << " sign=" << sign);
135+ "Setting new pid values for motor" << motor_index << ": max_pwm="<< max_pwm <<" sgleftref=" << sg_left << " sgrightref=" << sg_right << " f=" << f << " p=" << p << " i=" << i << " d="<< d << " imax=" << imax << " deadband="<< deadband << " sign=" << sign);
136
137 //the vector is of the size of the TO_MOTOR_DATA_TYPE enum.
138 //the value of the element at a given index is the value

Subscribers

People subscribed via source and target branches

to all changes: