icub-test
Loading...
Searching...
No Matches
Public Member Functions
PositionControlAccuracy Class Reference
A list of available tests

This tests checks the a position PID response, sending a step reference signal with a positionDirect command. More...

#include <PositionControlAccuracy.h>

Inherits yarp::robottestingframework::TestCase.

Public Member Functions

virtual bool setup (yarp::os::Property &property)
 
virtual void tearDown ()
 
virtual void run ()
 
bool goHome ()
 
void executeCmd ()
 
void setMode (int desired_mode)
 
void saveToFile (std::string filename, yarp::os::Bottle &b)
 

Detailed Description

This tests checks the a position PID response, sending a step reference signal with a positionDirect command.

This test currently does not return any error report. It simply moves a joint, and saves data to a different file for each joint. The data acquired can be analyzed with a Matlab script to evaluate the position PID properties. Be aware that a step greater than 5 degrees at the maximum speed can be dangerous for both the robot and the human operator!

example: testRunner -v -t PositionControlAccuracy.dll -p "--robot icubSim --part head --joints ""(0 1 2)"" --zeros ""(0 0 0)"" --step 5 --cycles 10 --sampleTime 0.010" example: testRunner -v -t PositionControlAccuracy.dll -p "--robot icubSim --part head --joints ""(2)"" --zeros ""(0)"" --step 5 --cycles 10 --sampleTime 0.010"

Accepts the following parameters:

Parameter name Type Units Default Value Required Description Notes
robot string - - Yes The name of the robot. e.g. icub
part string - - Yes The name of the robot part. e.g. left_arm
joints vector of ints - - Yes List of joints to be tested
zeros double deg - Yes The home position for each joint
cycles int - - Yes Each joint will be tested multiple times
step double deg - Yes The amplitude of the step reference signal Recommended max: 5 deg!
sampleTime double s - Yes The sample time of the control thread
home_tolerance double deg 0.5 No The max acceptable position error during the homing phase.
filename string No The output filename. If not specified, the name will be generated using 'part' parameter and joint number
step_duration double s No The duration of the step. After this time, a new test cycle starts.

Definition at line 55 of file PositionControlAccuracy.h.

Constructor & Destructor Documentation

◆ PositionControlAccuracy()

PositionControlAccuracy::PositionControlAccuracy ( )

Definition at line 41 of file PositionControlAccuracy.cpp.

41 : yarp::robottestingframework::TestCase("PositionControlAccuracy") {
42 m_jointsList = 0;
43 m_encoders = 0;
44 m_zeros = 0;
45 dd=0;
46 ipos=0;
47 icmd=0;
48 iimd=0;
49 ienc=0;
50 idir=0;
51 m_home_tolerance=0.5;
52 m_step_duration=4;
53}

◆ ~PositionControlAccuracy()

PositionControlAccuracy::~PositionControlAccuracy ( )
virtual

Definition at line 55 of file PositionControlAccuracy.cpp.

55{ }

Member Function Documentation

◆ goHome()

bool PositionControlAccuracy::goHome ( )

Definition at line 192 of file PositionControlAccuracy.cpp.

193{
194 for (int i = 0; i<m_n_cmd_joints; i++)
195 {
196 ipos->setRefSpeed(m_jointsList[i], 20.0);
197 ipos->positionMove(m_jointsList[i], m_zeros[i]);
198 }
199
200 int timeout = 0;
201 while (1)
202 {
203 int in_position=0;
204 for (int i = 0; i<m_n_cmd_joints; i++)
205 {
206 ienc->getEncoder(m_jointsList[i], &m_encoders[m_jointsList[i]]);
207 if (fabs(m_encoders[m_jointsList[i]] - m_zeros[i])<m_home_tolerance) in_position++;
208 }
209 if (in_position == m_n_cmd_joints) break;
210 if (timeout>100)
211 {
212 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching zero position");
213 return false;
214 }
215 yarp::os::Time::delay(0.2);
216 timeout++;
217 }
218 //sleep some additional time to complete movement from m_home_tolerance to 0
219 yarp::os::Time::delay(0.5);
220 return true;
221}

◆ run()

void PositionControlAccuracy::run ( )
virtual

Definition at line 223 of file PositionControlAccuracy.cpp.

224{
225 for (int i = 0; i < m_n_cmd_joints; i++)
226 {
227 for (int cycle = 0; cycle < m_cycles; cycle++)
228 {
229 ipid->setPid(VOCAB_PIDTYPE_POSITION,m_jointsList[i],m_orig_pid);
230 setMode(VOCAB_CM_POSITION);
231 if (goHome() == false)
232 {
233 ROBOTTESTINGFRAMEWORK_ASSERT_FAIL("Test stopped");
234 };
235
236 ipid->setPid(VOCAB_PIDTYPE_POSITION,m_jointsList[i],m_new_pid);
237 setMode(VOCAB_CM_POSITION_DIRECT);
238 double start_time = yarp::os::Time::now();
239
240 char cbuff[64];
241 sprintf(cbuff, "Testing Joint: %d cycle: %d", i, cycle);
242
243 std::string buff(cbuff);
244 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
245
246 double time_zero = 0;
247 yarp::os::Bottle dataToPlotRaw;
248 yarp::os::Bottle dataToPlotSync;
249
250 while (1)
251 {
252 double curr_time = yarp::os::Time::now();
253 double elapsed = curr_time - start_time;
254
255 if (elapsed <= 1.0)
256 {
257 m_cmd_single = m_zeros[i]; //0.0;
258 }
259 else if (elapsed > 1.0 && elapsed <= m_step_duration)
260 {
261 m_cmd_single = m_zeros[i] + m_step;
262 if (time_zero == 0) time_zero = elapsed;
263 }
264 else
265 {
266 break;
267 }
268
269 ienc->getEncoders(m_encoders);
270 idir->setPosition(m_jointsList[i], m_cmd_single);
271
272 Bottle& b1 = dataToPlotRaw.addList();
273 b1.addInt32(cycle);
274 b1.addFloat64(elapsed);
275 b1.addFloat64(m_encoders[m_jointsList[i]]);
276 b1.addFloat64(m_cmd_single);
277 yarp::os::Time::delay(m_sampleTime);
278 }
279
280 //reorder data
281 for (int t = 0; t < dataToPlotRaw.size(); t++)
282 {
283 int cycle = dataToPlotRaw.get(t).asList()->get(0).asInt32();
284 double time = dataToPlotRaw.get(t).asList()->get(1).asFloat64();
285 double val = dataToPlotRaw.get(t).asList()->get(2).asFloat64();
286 double cmd = dataToPlotRaw.get(t).asList()->get(3).asFloat64();
287 Bottle& b1 = dataToPlotSync.addList();
288 b1.addInt32(cycle);
289 b1.addFloat64(time - time_zero);
290 b1.addFloat64(val);
291 b1.addFloat64(cmd);
292 }
293
294 m_dataToSave.append(dataToPlotSync);
295 } //cycle loop
296
297 //save data
298 std::string filename;
299 if (m_requested_filename=="")
300 {
301 char cfilename[128];
302 sprintf(cfilename, "positionControlAccuracy_plot_%s%d.txt", m_partName.c_str(), i);
303 filename = cfilename;
304 //filename += m_partName;
305 //filename += std::to_string(i);
306 //filename += ".txt";
307 }
308 else
309 {
310 filename=m_requested_filename;
311 }
312 yInfo() << "Saving file to: "<< filename;
313 saveToFile(filename, m_dataToSave);
314 ipid->setPid(VOCAB_PIDTYPE_POSITION,m_jointsList[i],m_orig_pid);
315 } //joint loop
316
317 //data acquisition ends here
318 setMode(VOCAB_CM_POSITION);
319 goHome();
320 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Data acquisition complete");
321
322 //plot data
323 /*for (int i = 0; i < m_n_cmd_joints; i++)
324 {
325 std::string filename = "positionControlAccuracy_plot_";
326 filename += m_partName;
327 filename += std::to_string(i);
328 filename += ".txt";
329 char plotstring[1000];
330 sprintf(plotstring, "gnuplot -e \" unset key; plot for [col=1:%d] '%s' using col with lines \" -persist", m_n_cmd_joints, filename.c_str());
331 system(plotstring);
332 }*/
333}

◆ saveToFile()

void PositionControlAccuracy::saveToFile ( std::string  filename,
yarp::os::Bottle &  b 
)

Definition at line 335 of file PositionControlAccuracy.cpp.

336{
337 std::fstream fs;
338 fs.open(filename.c_str(), std::fstream::out);
339
340 for (int i = 0; i<b.size(); i++)
341 {
342 std::string s = b.get(i).toString();
343 std::replace(s.begin(), s.end(), '(', ' ');
344 std::replace(s.begin(), s.end(), ')', ' ');
345 fs << s << std::endl;
346 }
347
348 fs.close();
349}

◆ setMode()

void PositionControlAccuracy::setMode ( int  desired_mode)

Definition at line 160 of file PositionControlAccuracy.cpp.

161{
162 for (int i = 0; i<m_n_cmd_joints; i++)
163 {
164 icmd->setControlMode(m_jointsList[i], desired_mode);
165 iimd->setInteractionMode(m_jointsList[i], VOCAB_IM_STIFF);
166 yarp::os::Time::delay(0.010);
167 }
168
169 int cmode;
170 yarp::dev::InteractionModeEnum imode;
171 int timeout = 0;
172
173 while (1)
174 {
175 int ok=0;
176 for (int i = 0; i<m_n_cmd_joints; i++)
177 {
178 icmd->getControlMode(m_jointsList[i], &cmode);
179 iimd->getInteractionMode(m_jointsList[i], &imode);
180 if (cmode==desired_mode && imode==VOCAB_IM_STIFF) ok++;
181 }
182 if (ok == m_n_cmd_joints) break;
183 if (timeout>100)
184 {
185 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set control mode/interaction mode");
186 }
187 yarp::os::Time::delay(0.2);
188 timeout++;
189 }
190}

◆ setup()

bool PositionControlAccuracy::setup ( yarp::os::Property &  property)
virtual

Definition at line 57 of file PositionControlAccuracy.cpp.

57 {
58
59 //updating the test name
60 if(property.check("name"))
61 setName(property.find("name").asString());
62
63 // updating parameters
64 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
65 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
66 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
67 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("zeros"), "The zero position list must be given as the test parameter!");
68 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("cycles"), "The number of cycles of the control signal must be given as the test parameter!");
69 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("step"), "The amplitude of the step reference signal expressed in degrees!");
70 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("sampleTime"), "The sampleTime of the control signal must be given as the test parameter!");
71 if(property.check("filename"))
72 {m_requested_filename = property.find("filename").asString();}
73 if(property.check("home_tolerance"))
74 {m_home_tolerance = property.find("home_tolerance").asFloat64();}
75 if(property.check("step_duration"))
76 {m_step_duration = property.find("step_duration").asFloat64();}
77
78 m_robotName = property.find("robot").asString();
79 m_partName = property.find("part").asString();
80
81 Bottle* jointsBottle = property.find("joints").asList();
82 Bottle* zerosBottle = property.find("zeros").asList();
83
84 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
85 m_n_cmd_joints = jointsBottle->size();
86 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(m_n_cmd_joints>0, "invalid number of joints, it must be >0");
87
88 m_step = property.find("step").asFloat64();
89
90 m_cycles = property.find("cycles").asInt32();
91 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(m_cycles>0, "invalid cycles");
92
93 m_sampleTime = property.find("sampleTime").asFloat64();
94 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(m_sampleTime>0, "invalid sampleTime");
95
96 Property options;
97 options.put("device", "remote_controlboard");
98 options.put("remote", "/" + m_robotName + "/" + m_partName);
99 options.put("local", "/positionControlAccuracyTest/" + m_robotName + "/" + m_partName);
100
101 dd = new PolyDriver(options);
102 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
103 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(idir),"Unable to open position direct interface");
104 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
105 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
106 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
107 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
108 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipid),"Unable to open pid interface");
109
110 if (!ienc->getAxes(&m_n_part_joints))
111 {
112 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
113 }
114
115 m_zeros = new double[m_n_part_joints];
116 m_encoders = new double[m_n_part_joints];
117 m_jointsList = new int[m_n_cmd_joints];
118 for (int i = 0; i <m_n_cmd_joints; i++) m_jointsList[i] = jointsBottle->get(i).asInt32();
119 for (int i = 0; i <m_n_cmd_joints; i++) m_zeros[i] = zerosBottle->get(i).asFloat64();
120
121 double p_Kp=std::nanf("");
122 double p_Ki=std::nanf("");
123 double p_Kd=std::nanf("");
124 double p_Max=std::nanf("");
125
126 int cj=m_jointsList[0];
127 ipid->getPid(VOCAB_PIDTYPE_POSITION,cj,&m_orig_pid);
128 m_new_pid=m_orig_pid;
129
130 if(property.check("Kp"))
131 {p_Kp = property.find("Kp").asFloat64();}
132 if(property.check("Ki"))
133 {p_Ki = property.find("Ki").asFloat64();}
134 if(property.check("Kd"))
135 {p_Kd = property.find("Kd").asFloat64();}
136 //if(property.check("MaxValue"))
137 // {p_Max = property.find("MaxValue").asFloat64();}
138 if (std::isnan(p_Kp)==false) {m_new_pid.kp=p_Kp;}
139 if (std::isnan(p_Kd)==false) {m_new_pid.kd=p_Kd;}
140 if (std::isnan(p_Ki)==false) {m_new_pid.ki=p_Ki;}
141
142 /*if (std::isnan(p_Kp)==false ||
143 std::isnan(p_Ki)==false ||
144 std::isnan(p_Kd)==false)
145 {
146 ipid->setPid(cj,m_new_pid);
147 }*/
148
149 return true;
150}

◆ tearDown()

void PositionControlAccuracy::tearDown ( )
virtual

Definition at line 152 of file PositionControlAccuracy.cpp.

153{
154 if (m_jointsList) { delete [] m_jointsList; m_jointsList = 0; }
155 if (m_zeros) { delete [] m_zeros; m_zeros = 0; }
156 if (m_encoders) { delete [] m_encoders; m_encoders = 0; }
157 if (dd) {delete dd; dd =0;}
158}
The documentation for this class was generated from the following files:
Generated by doxygen 1.9.8