icub-test
•All Data Structures Modules Pages
TorqueControlConsistency.cpp
1 /*
2  * iCub Robot Unit Tests (Robot Testing Framework)
3  *
4  * Copyright (C) 2015-2019 Istituto Italiano di Tecnologia (IIT)
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19  */
20 
21 #include <math.h>
22 #include <robottestingframework/TestAssert.h>
23 #include <robottestingframework/dll/Plugin.h>
24 #include <yarp/os/Time.h>
25 #include <yarp/os/Property.h>
26 
27 #include "TorqueControlConsistency.h"
28 
29 //example1 -v -t TorqueControlConsistency.dll -p "--robot icub --part head --joints ""(0)"" --zero 0"
30 //example2 -v -t TorqueControlConsistency.dll -p "--robot icub --part head --joints ""(0 1 2)"" --zero 0 "
31 //example3 -v -t TorqueControlConsistency.dll -p "--robot icub --part head --joints ""(0 1 2 3 4 5)"" --zero 0"
32 
33 using namespace robottestingframework;
34 using namespace yarp::os;
35 using namespace yarp::dev;
36 
37 // prepare the plugin
38 ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(TorqueControlConsistency)
39 
40 TorqueControlConsistency::TorqueControlConsistency() : yarp::robottestingframework::TestCase("TorqueControlConsistency") {
41  jointsList=0;
42  pos_tot=0;
43  dd=0;
44  ipos=0;
45  iamp=0;
46  icmd=0;
47  iimd=0;
48  ienc=0;
49  itrq=0;
50  cmd_some=0;
51  cmd_tot=0;
52  prevcurr_some=0;
53  prevcurr_tot=0;
54 }
55 
56 TorqueControlConsistency::~TorqueControlConsistency() { }
57 
58 bool TorqueControlConsistency::setup(yarp::os::Property& property) {
59 
60  //updating the test name
61  if(property.check("name"))
62  setName(property.find("name").asString());
63 
64  // updating parameters
65  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
66  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
67  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
68  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("zero"), "The zero position must be given as the test parameter!");
69 
70  robotName = property.find("robot").asString();
71  partName = property.find("part").asString();
72 
73  zero = property.find("zero").asFloat64();
74 
75  Bottle* jointsBottle = property.find("joints").asList();
76  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
77  n_cmd_joints = jointsBottle->size();
78  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(n_cmd_joints>0,"invalid number of joints, it must be >0");
79 
80  Property options;
81  options.put("device", "remote_controlboard");
82  options.put("remote", "/"+robotName+"/"+partName);
83  options.put("local", "/TorqueControlConsistencyTest/"+robotName+"/"+partName);
84 
85  dd = new PolyDriver(options);
86  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
87  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(itrq),"Unable to open torque control interface");
88  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
89  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iamp),"Unable to open ampliefier interface");
90  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
91  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
92  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
93 
94  if (!ienc->getAxes(&n_part_joints))
95  {
96  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
97  }
98 
99  if (n_part_joints<=0)
100  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Error this part has in invalid (<=0) number of jonits");
101  else if (jointsBottle->size() == 1)
102  cmd_mode=single_joint;
103  else if (jointsBottle->size() < n_part_joints)
104  cmd_mode=some_joints;
105  else if (jointsBottle->size() == n_part_joints)
106  cmd_mode=all_joints;
107  else
108  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("invalid joint selection?");
109 
110  cmd_tot = new double[n_part_joints];
111  pos_tot=new double[n_part_joints];
112  jointsList=new int[n_cmd_joints];
113  cmd_some=new double[n_cmd_joints];
114  prevcurr_tot=new double[n_part_joints];
115  prevcurr_some=new double[n_cmd_joints];
116  for (int i=0; i <n_cmd_joints; i++) jointsList[i]=jointsBottle->get(i).asInt32();
117 
118  return true;
119 }
120 
121 void TorqueControlConsistency::tearDown()
122 {
123  if (jointsList) {delete jointsList; jointsList =0;}
124  if (dd) {delete dd; dd =0;}
125 }
126 
127 void TorqueControlConsistency::setMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
128 {
129  for (int i=0; i<n_cmd_joints; i++)
130  {
131  icmd->setControlMode(jointsList[i],desired_control_mode);
132  iimd->setInteractionMode(jointsList[i],desired_interaction_mode);
133  yarp::os::Time::delay(0.010);
134  }
135 }
136 
137 void TorqueControlConsistency::verifyMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, std::string title)
138 {
139  int cmode;
140  yarp::dev::InteractionModeEnum imode;
141  int timeout = 0;
142 
143  while (1)
144  {
145  int ok=0;
146  for (int i=0; i<n_cmd_joints; i++)
147  {
148  icmd->getControlMode (jointsList[i],&cmode);
149  iimd->getInteractionMode(jointsList[i],&imode);
150  if (cmode==desired_control_mode && imode==desired_interaction_mode) ok++;
151  }
152  if (ok==n_cmd_joints) break;
153  if (timeout>100)
154  {
155  char sbuf[500];
156  sprintf(sbuf,"Test (%s) failed: current mode is (%d,%d), it should be (%d,%d)",title.c_str(), desired_control_mode,desired_interaction_mode,cmode,imode);
157  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
158  }
159  yarp::os::Time::delay(0.2);
160  timeout++;
161  }
162  char sbuf[500];
163  sprintf(sbuf,"Test (%s) passed: current mode is (%d,%d)",title.c_str(), desired_control_mode,desired_interaction_mode);
164  ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
165 }
166 
167 void TorqueControlConsistency::goHome()
168 {
169  for (int i=0; i<n_cmd_joints; i++)
170  {
171  ipos->setRefSpeed(jointsList[i],20.0);
172  ipos->positionMove(jointsList[i],zero);
173  }
174 
175  int timeout = 0;
176  while (1)
177  {
178  int in_position=0;
179  for (int i=0; i<n_cmd_joints; i++)
180  {
181  ienc->getEncoder(jointsList[i],&pos_tot[jointsList[i]]);
182  if (fabs(pos_tot[jointsList[i]]-zero)<0.5) in_position++;
183  }
184  if (in_position==n_cmd_joints) break;
185  if (timeout>100)
186  {
187  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching zero position");
188  }
189  yarp::os::Time::delay(0.2);
190  timeout++;
191  }
192 }
193 
194 void TorqueControlConsistency::setRefTorque(double value)
195 {
196  cmd_single=value;
197  if (cmd_mode==single_joint)
198  {
199  for (int i=0; i<n_cmd_joints; i++)
200  {
201  itrq->setRefTorque(jointsList[i],cmd_single);
202  }
203  }
204  else if (cmd_mode==some_joints)
205  {
206  //same of single_joint, since multiple joint is not currently supported
207  for (int i=0; i<n_cmd_joints; i++)
208  {
209  itrq->setRefTorque(jointsList[i],cmd_single);
210  }
211  }
212  else if (cmd_mode==all_joints)
213  {
214  for (int i=0; i<n_part_joints; i++)
215  {
216  cmd_tot[i]=cmd_single;
217  }
218  itrq->setRefTorques(cmd_tot);
219  }
220  else
221  {
222  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid cmd_mode");
223  }
224  yarp::os::Time::delay(0.010);
225 }
226 
227 void TorqueControlConsistency::verifyRefTorque(double verify_val, std::string title)
228 {
229  double value;
230  char sbuf[500];
231  if (cmd_mode==single_joint)
232  {
233  for (int i=0; i<n_cmd_joints; i++)
234  {
235  itrq->getRefTorque(jointsList[i],&value);
236  if (value==verify_val)
237  {
238  sprintf(sbuf,"Test (%s) passed, j%d current reference is (%f)",title.c_str(),i, verify_val);
239  ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
240  }
241  else
242  {
243  sprintf(sbuf,"Test (%s) failed: current reference is (%f), it should be (%f)",title.c_str(), value, verify_val);
244  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
245  }
246  }
247  }
248  else if (cmd_mode==some_joints)
249  {
250  //same of single_joint, since multiple joint is not currently supported
251  for (int i=0; i<n_cmd_joints; i++)
252  {
253  itrq->getRefTorque(jointsList[i],&value);
254  if (value==verify_val)
255  {
256  sprintf(sbuf,"Test (%s) passed j%d current reference is (%f)",title.c_str(),i, verify_val);
257  ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
258  }
259  else
260  {
261  sprintf(sbuf,"Test (%s) failed: current reference is (%f), it should be (%f)",title.c_str(), value, verify_val);
262  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
263  }
264  }
265  }
266  else if (cmd_mode==all_joints)
267  {
268  int ok=0;
269  itrq->getRefTorques(cmd_tot);
270  for (int i=0; i<n_part_joints; i++)
271  {
272  if (verify_val==cmd_tot[i]) ok++;
273  }
274  if (ok==n_part_joints)
275  {
276  sprintf(sbuf,"Test (%s) passed, current reference is (%f)",title.c_str(), verify_val);
277  ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
278  }
279  else
280  {
281  sprintf(sbuf,"Test (%s) failed: only %d joints (of %d) are ok",title.c_str(),ok,n_part_joints);
282  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
283  }
284  }
285  else
286  {
287  ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid cmd_mode");
288  }
289  yarp::os::Time::delay(0.010);
290 }
291 
292 void TorqueControlConsistency::run()
293 {
294  setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
295  verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test0");
296  goHome();
297 
298  setMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF);
299  verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test1");
300  setRefTorque(0.1);
301  verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test2");
302  verifyRefTorque(0.1,"test2a");
303  verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test2b");
304 
305  setRefTorque(0);
306  verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test3");
307  verifyRefTorque(0,"test3a");
308  verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test3b");
309  setRefTorque(-0.1);
310  verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test4");
311  verifyRefTorque(-0.1,"test4a");
312  verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test4b");
313 
314  setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
315  verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test5");
316  goHome();
317 
318 }
Generated by doxygen 1.9.1