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

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

#include <TorqueControlAccuracy.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 torque PID response, sending a step reference signal with a setRefTorque 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 analized with a matalab script to evaluate the torque PID properties. Be aware that a step greater than 1 Nm may be dangerous for both the robot and the human operator!

example: testRunner -v -t TorqueControlAccuracy.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 TorqueControlAccuracy.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 Nm - Yes The amplitude of the step reference signal Recommended max: 1 Nm!
sampleTime double s - Yes The sample time of the control thread

Definition at line 52 of file TorqueControlAccuracy.h.

Constructor & Destructor Documentation

◆ TorqueControlAccuracy()

TorqueControlAccuracy::TorqueControlAccuracy ( )

Definition at line 39 of file TorqueControlAccuracy.cpp.

39 : yarp::robottestingframework::TestCase("TorqueControlAccuracy") {
40 m_jointsList = 0;
41 m_encoders = 0;
42 m_torques = 0;
43 m_zeros = 0;
44 dd=0;
45 ipos=0;
46 icmd=0;
47 iimd=0;
48 ienc=0;
49 itrq=0;
50}

◆ ~TorqueControlAccuracy()

TorqueControlAccuracy::~TorqueControlAccuracy ( )
virtual

Definition at line 52 of file TorqueControlAccuracy.cpp.

52{ }

Member Function Documentation

◆ goHome()

bool TorqueControlAccuracy::goHome ( )

Definition at line 156 of file TorqueControlAccuracy.cpp.

157{
158 for (int i = 0; i<m_n_cmd_joints; i++)
159 {
160 ipos->setRefSpeed(m_jointsList[i], 20.0);
161 ipos->positionMove(m_jointsList[i], m_zeros[i]);
162 }
163
164 int timeout = 0;
165 while (1)
166 {
167 int in_position=0;
168 for (int i = 0; i<m_n_cmd_joints; i++)
169 {
170 ienc->getEncoder(m_jointsList[i], &m_encoders[m_jointsList[i]]);
171 if (fabs(m_encoders[m_jointsList[i]] - m_zeros[i])<0.5) in_position++;
172 }
173 if (in_position == m_n_cmd_joints) break;
174 if (timeout>100)
175 {
176 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching zero position");
177 return false;
178 }
179 yarp::os::Time::delay(0.2);
180 timeout++;
181 }
182 //sleep some additional time to complete movement from 0.5 to 0
183 yarp::os::Time::delay(0.5);
184 return true;
185}

◆ run()

void TorqueControlAccuracy::run ( )
virtual

Definition at line 187 of file TorqueControlAccuracy.cpp.

188{
189 for (int i = 0; i < m_n_cmd_joints; i++)
190 {
191 for (int cycle = 0; cycle < m_cycles; cycle++)
192 {
193 setMode(VOCAB_CM_POSITION);
194 if (goHome() == false)
195 {
196 ROBOTTESTINGFRAMEWORK_ASSERT_FAIL("Test stopped");
197 };
198 setMode(VOCAB_CM_TORQUE);
199 double start_time = yarp::os::Time::now();
200
201 std::string buff = "Testing Joint: " + std::to_string(i) + " cycle: " + std::to_string(cycle);
202 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
203
204 double time_zero = 0;
205 yarp::os::Bottle dataToPlotRaw;
206 yarp::os::Bottle dataToPlotSync;
207
208 while (1)
209 {
210 double curr_time = yarp::os::Time::now();
211 double elapsed = curr_time - start_time;
212
213 if (elapsed <= 1.0)
214 {
215 m_cmd_single = 0.0;
216 }
217 else if (elapsed > 1.0 && elapsed <= 4.0)
218 {
219 m_cmd_single = m_zeros[i] + m_step;
220 if (time_zero == 0) time_zero = elapsed;
221 }
222 else
223 {
224 break;
225 }
226
227 ienc->getEncoders(m_encoders);
228 itrq->getTorques(m_torques);
229 itrq->setRefTorque(m_jointsList[i], m_cmd_single);
230
231 Bottle& b1 = dataToPlotRaw.addList();
232 b1.addInt32(cycle);
233 b1.addFloat64(elapsed);
234 b1.addFloat64(m_torques[m_jointsList[i]]);
235 b1.addFloat64(m_cmd_single);
236 yarp::os::Time::delay(m_sampleTime);
237 }
238
239 //reorder data
240 for (int t = 0; t < dataToPlotRaw.size(); t++)
241 {
242 int cycle = dataToPlotRaw.get(t).asList()->get(0).asInt32();
243 double time = dataToPlotRaw.get(t).asList()->get(1).asFloat64();
244 double val = dataToPlotRaw.get(t).asList()->get(2).asFloat64();
245 double cmd = dataToPlotRaw.get(t).asList()->get(3).asFloat64();
246 Bottle& b1 = dataToPlotSync.addList();
247 b1.addInt32(cycle);
248 b1.addFloat64(time - time_zero);
249 b1.addFloat64(val);
250 b1.addFloat64(cmd);
251 }
252
253 m_dataToSave.append(dataToPlotSync);
254 } //cycle loop
255
256 //save data
257 std::string filename = "torqueControlAccuracy_plot_";
258 filename += m_partName;
259 filename += std::to_string(i);
260 filename += ".txt";
261 saveToFile(filename, m_dataToSave);
262 } //joint loop
263
264 //data acquisition ends here
265 setMode(VOCAB_CM_POSITION);
266 goHome();
267 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Data acquisition complete");
268
269 //plot data
270 /*for (int i = 0; i < m_n_cmd_joints; i++)
271 {
272 std::string filename = "torqueControlAccuracy_plot_";
273 filename += m_partName;
274 filename += std::to_string(i);
275 filename += ".txt";
276 char plotstring[1000];
277 sprintf(plotstring, "gnuplot -e \" unset key; plot for [col=1:%d] '%s' using col with lines \" -persist", m_n_cmd_joints, filename.c_str());
278 system(plotstring);
279 }*/
280}

◆ saveToFile()

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

Definition at line 282 of file TorqueControlAccuracy.cpp.

283{
284 std::fstream fs;
285 fs.open(filename.c_str(), std::fstream::out);
286
287 for (int i = 0; i<b.size(); i++)
288 {
289 std::string s = b.get(i).toString();
290 std::replace(s.begin(), s.end(), '(', ' ');
291 std::replace(s.begin(), s.end(), ')', ' ');
292 fs << s << std::endl;
293 }
294
295 fs.close();
296}

◆ setMode()

void TorqueControlAccuracy::setMode ( int  desired_mode)

Definition at line 124 of file TorqueControlAccuracy.cpp.

125{
126 for (int i = 0; i<m_n_cmd_joints; i++)
127 {
128 icmd->setControlMode(m_jointsList[i], desired_mode);
129 iimd->setInteractionMode(m_jointsList[i], VOCAB_IM_STIFF);
130 yarp::os::Time::delay(0.010);
131 }
132
133 int cmode;
134 yarp::dev::InteractionModeEnum imode;
135 int timeout = 0;
136
137 while (1)
138 {
139 int ok=0;
140 for (int i = 0; i<m_n_cmd_joints; i++)
141 {
142 icmd->getControlMode(m_jointsList[i], &cmode);
143 iimd->getInteractionMode(m_jointsList[i], &imode);
144 if (cmode==desired_mode && imode==VOCAB_IM_STIFF) ok++;
145 }
146 if (ok == m_n_cmd_joints) break;
147 if (timeout>100)
148 {
149 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set control mode/interaction mode");
150 }
151 yarp::os::Time::delay(0.2);
152 timeout++;
153 }
154}

◆ setup()

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

Definition at line 54 of file TorqueControlAccuracy.cpp.

54 {
55
56 //updating the test name
57 if(property.check("name"))
58 setName(property.find("name").asString());
59
60 // updating parameters
61 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(property.check("robot"), "The robot name must be given as the test parameter!");
62 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(property.check("part"), "The part name must be given as the test parameter!");
63 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(property.check("joints"), "The joints list must be given as the test parameter!");
64 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(property.check("zeros"), "The zero position list must be given as the test parameter!");
65 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(property.check("cycles"), "The number of cycles of the control signal must be given as the test parameter!");
66 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(property.check("step"), "The amplitude of the step reference signal expressed in Nm!");
67 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(property.check("sampleTime"), "The sampleTime of the control signal must be given as the test parameter!");
68
69 m_robotName = property.find("robot").asString();
70 m_partName = property.find("part").asString();
71
72 Bottle* jointsBottle = property.find("joints").asList();
73 Bottle* zerosBottle = property.find("zeros").asList();
74
75 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(jointsBottle!=0,"unable to parse joints parameter");
76 m_n_cmd_joints = jointsBottle->size();
77 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(m_n_cmd_joints>0, "invalid number of joints, it must be >0");
78
79 m_step = property.find("step").asFloat64();
80
81 m_cycles = property.find("cycles").asInt32();
82 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(m_cycles>0, "invalid cycles");
83
84 m_sampleTime = property.find("sampleTime").asFloat64();
85 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(m_sampleTime>0, "invalid sampleTime");
86
87 Property options;
88 options.put("device", "remote_controlboard");
89 options.put("remote", "/" + m_robotName + "/" + m_partName);
90 options.put("local", "/TorqueControlAccuracyTest/" + m_robotName + "/" + m_partName);
91
92 dd = new PolyDriver(options);
93 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(dd->isValid(),"Unable to open device driver");
94 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(dd->view(itrq),"Unable to open torque control interface");
95 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(dd->view(ienc),"Unable to open encoders interface");
96 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(dd->view(ipos),"Unable to open position interface");
97 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(dd->view(icmd),"Unable to open control mode interface");
98 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF(dd->view(iimd),"Unable to open interaction mode interface");
99
100 if (!ienc->getAxes(&m_n_part_joints))
101 {
102 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
103 }
104
105 m_zeros = new double[m_n_part_joints];
106 m_encoders = new double[m_n_part_joints];
107 m_jointsList = new int[m_n_cmd_joints];
108 m_torques = new double[m_n_part_joints];
109 for (int i = 0; i <m_n_cmd_joints; i++) m_jointsList[i] = jointsBottle->get(i).asInt32();
110 for (int i = 0; i <m_n_cmd_joints; i++) m_zeros[i] = zerosBottle->get(i).asFloat64();
111
112 return true;
113}

◆ tearDown()

void TorqueControlAccuracy::tearDown ( )
virtual

Definition at line 115 of file TorqueControlAccuracy.cpp.

116{
117 if (m_jointsList) { delete [] m_jointsList; m_jointsList = 0; }
118 if (m_zeros) { delete [] m_zeros; m_zeros = 0; }
119 if (m_torques) { delete [] m_torques; m_torques = 0; }
120 if (m_encoders) { delete [] m_encoders; m_encoders = 0; }
121 if (dd) {delete dd; dd =0;}
122}
The documentation for this class was generated from the following files:
Generated by doxygen 1.9.8