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

This tests checks the positionDirect control, sending a sinusoidal reference signal, with parametric frequency and amplitude. More...

#include <PositionDirect.h>

Inherits yarp::robottestingframework::TestCase.

Public Member Functions

virtual bool setup (yarp::os::Property &property)
 
virtual void tearDown ()
 
virtual void run ()
 
void goHome ()
 
void executeCmd ()
 
void setMode (int desired_mode)
 

Detailed Description

This tests checks the positionDirect control, sending a sinusoidal reference signal, with parametric frequency and amplitude.

The sample time, typical in the range of 10 ms, can be also be adjusted by the user. This test currently does not return any error report. It simply moves a joint, and the user visually evaluates the smoothness of the performed trajectory. In the future automatic checks/plots may be added to the test. Be aware theat may exists set of parameters (e.g. high values of sample time / ampiltude /frequency) that may lead to PID instability and damage the joint. The test is able to check all the three types of yarp methods (single joint, multi joint, all joints), depending on the value of cmdMode Parameter.

example: testRunner -v -t PositionDirect.dll - p "--robot icub --part head --joints ""(0 1 2)"" --zero 0 --frequency 0.8 --amplitude 10.0 --cycles 10 --tolerance 1.0 --sampleTime 0.010 --cmdMode 0" example: testRunner -v -t PositionDirect.dll - p "--robot icub --part head --joints ""(2)"" --zero 0 --frequency 0.4 --amplitude 10.0 --cycles 10 --tolerance 1.0 --sampleTime 0.010 --cmdMode 0"

Check the following functions:

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 trhe robot part. e.g. left_arm
joints vector of ints - - Yes List of joints to be tested
zero double deg - Yes The home position for each joint
cycles int - - Yes The number of test cycles (going from max to min position and viceversa)
frequency double deg - Yes The frequency of the sine reference signal
amplitude double deg - Yes The ampiltude of the sine reference signal
tolerance double deg - Yes The tolerance used when moving from min to max reference position and viceversa
sampleTime double s - Yes The sample time of the control thread
cmdMode int deg - Yes = 0 to test single joint method, = 1 to test all joints, = 2 to test multi joint method

Definition at line 60 of file PositionDirect.h.

Constructor & Destructor Documentation

◆ PositionDirect()

PositionDirect::PositionDirect ( )

Definition at line 36 of file PositionDirect.cpp.

36 : yarp::robottestingframework::TestCase("PositionDirect") {
37 jointsList=0;
38 pos_tot=0;
39 dd=0;
40 ipos=0;
41 icmd=0;
42 iimd=0;
43 ienc=0;
44 idir=0;
45 cmd_some=0;
46 cmd_tot=0;
47}

◆ ~PositionDirect()

PositionDirect::~PositionDirect ( )
virtual

Definition at line 49 of file PositionDirect.cpp.

49{ }

Member Function Documentation

◆ executeCmd()

void PositionDirect::executeCmd ( )

Definition at line 172 of file PositionDirect.cpp.

173{
174 if (cmd_mode==single_joint)
175 {
176 for (int i=0; i<n_cmd_joints; i++)
177 {
178 idir->setPosition(jointsList[i],cmd_single);
179 }
180 }
181 else if (cmd_mode==some_joints)
182 {
183 for (int i=0; i<n_cmd_joints; i++)
184 {
185 cmd_some[i]=cmd_single;
186 }
187 idir->setPositions(n_cmd_joints,jointsList, cmd_some);
188 }
189 else if (cmd_mode==all_joints)
190 {
191 for (int i=0; i<n_part_joints; i++)
192 {
193 cmd_tot[i]=cmd_single;
194 }
195 idir->setPositions(cmd_tot);
196 }
197 else
198 {
199 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid cmd_mode");
200 }
201
202 prev_cmd=cmd_single;
203}

◆ goHome()

void PositionDirect::goHome ( )

Definition at line 205 of file PositionDirect.cpp.

206{
207 for (int i=0; i<n_cmd_joints; i++)
208 {
209 ipos->setRefSpeed(jointsList[i],20.0);
210 ipos->positionMove(jointsList[i],zero);
211 }
212
213 int timeout = 0;
214 while (1)
215 {
216 int in_position=0;
217 for (int i=0; i<n_cmd_joints; i++)
218 {
219 ienc->getEncoder(jointsList[i],&pos_tot[jointsList[i]]);
220 if (fabs(pos_tot[jointsList[i]]-zero)<0.5) in_position++;
221 }
222 if (in_position==n_cmd_joints) break;
223 if (timeout>100)
224 {
225 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching zero position");
226 }
227 yarp::os::Time::delay(0.2);
228 timeout++;
229 }
230}

◆ run()

void PositionDirect::run ( )
virtual

Definition at line 232 of file PositionDirect.cpp.

233{
234 setMode(VOCAB_CM_POSITION);
235 goHome();
236 setMode(VOCAB_CM_POSITION_DIRECT);
237
238 double start_time = yarp::os::Time::now();
239 const double max_step = 2.0;
240 prev_cmd=cmd_single = amplitude*sin(0.0)+zero;
241 while(1)
242 {
243 double curr_time = yarp::os::Time::now();
244 double elapsed = curr_time-start_time;
245 cmd_single = amplitude*(2*3.14159265359*frequency*elapsed)+zero;
246
247 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(fabs(prev_cmd-cmd_single)<max_step,
248 Asserter::format("error in signal generation: previous: %+6.3f current: %+6.3f max step: %+6.3f",
249 prev_cmd,cmd_single,max_step));
250 ienc->getEncoders(pos_tot);
251 executeCmd();
252 //printf("%+6.3f %f\n",elapsed, cmd);
253 yarp::os::Time::delay(sampleTime);
254 if (elapsed*frequency>cycles) break;
255 }
256
257 setMode(VOCAB_CM_POSITION);
258 goHome();
259}

◆ setMode()

void PositionDirect::setMode ( int  desired_mode)

Definition at line 140 of file PositionDirect.cpp.

141{
142 for (int i=0; i<n_cmd_joints; i++)
143 {
144 icmd->setControlMode(jointsList[i],desired_mode);
145 iimd->setInteractionMode(jointsList[i],VOCAB_IM_STIFF);
146 yarp::os::Time::delay(0.010);
147 }
148
149 int cmode;
150 yarp::dev::InteractionModeEnum imode;
151 int timeout = 0;
152
153 while (1)
154 {
155 int ok=0;
156 for (int i=0; i<n_cmd_joints; i++)
157 {
158 icmd->getControlMode (jointsList[i],&cmode);
159 iimd->getInteractionMode(jointsList[i],&imode);
160 if (cmode==desired_mode && imode==VOCAB_IM_STIFF) ok++;
161 }
162 if (ok==n_cmd_joints) break;
163 if (timeout>100)
164 {
165 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set control mode/interaction mode");
166 }
167 yarp::os::Time::delay(0.2);
168 timeout++;
169 }
170}

◆ setup()

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

Definition at line 51 of file PositionDirect.cpp.

51 {
52
53 //updating the test name
54 if(property.check("name"))
55 setName(property.find("name").asString());
56
57 // updating parameters
58 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
59 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
60 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
61 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("zero"), "The zero position must be given as the test parameter!");
62 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("frequency"), "The frequency of the control signal must be given as the test parameter!");
63 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("amplitude"), "The amplitude of the control signal must be given as the test parameter!");
64 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("cycles"), "The number of cycles of the control signal must be given as the test parameter!");
65 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("tolerance"), "The tolerance of the control signal must be given as the test parameter!");
66 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("sampleTime"), "The sampleTime of the control signal must be given as the test parameter!");
67 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("cmdMode"), "the cmdType must be given as the test parameter! 0=single_joint, 1=all_joints, 2=some_joints");
68
69 robotName = property.find("robot").asString();
70 partName = property.find("part").asString();
71
72 Bottle* jointsBottle = property.find("joints").asList();
73 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
74 n_cmd_joints = jointsBottle->size();
75 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(n_cmd_joints>0,"invalid number of joints, it must be >0");
76
77 frequency = property.find("frequency").asFloat64();
78 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(frequency>0,"invalid frequency");
79
80 amplitude = property.find("amplitude").asFloat64();
81 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(amplitude>0,"invalid amplitude");
82
83 zero = property.find("zero").asFloat64();
84
85 cycles = property.find("cycles").asFloat64();
86 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(cycles>0,"invalid cycles");
87
88 tolerance = property.find("tolerance").asFloat64();
89 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tolerance>0,"invalid tolerance");
90
91 sampleTime = property.find("sampleTime").asFloat64();
92 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(sampleTime>0,"invalid sampleTime");
93
94 cmd_mode = (cmd_mode_t) property.find("cmdMode").asInt32();
95 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(cmd_mode>=0 && cmd_mode<=2,"invalid cmdMode: can be 0=single_joint, 1=all_joints ,2=some_joints");
96
97 Property options;
98 options.put("device", "remote_controlboard");
99 options.put("remote", "/"+robotName+"/"+partName);
100 options.put("local", "/positionDirectTest/"+robotName+"/"+partName);
101
102 dd = new PolyDriver(options);
103 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
104 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(idir),"Unable to open position direct interface");
105 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
106 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
107 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
108 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
109
110 if (!ienc->getAxes(&n_part_joints))
111 {
112 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
113 }
114
115 if (cmd_mode==all_joints && n_part_joints!=n_cmd_joints)
116 {
117 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("if all_joints=2 mode is selected, joints parameter must include the full list of joints");
118 }
119
120 if (cmd_mode==single_joint && n_cmd_joints!=1)
121 {
122 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("if single_joint=1 mode is selected, joints parameter must include one single joint");
123 }
124
125 cmd_tot = new double[n_part_joints];
126 pos_tot=new double[n_part_joints];
127 jointsList=new int[n_cmd_joints];
128 cmd_some=new double[n_cmd_joints];
129 for (int i=0; i <n_cmd_joints; i++) jointsList[i]=jointsBottle->get(i).asInt32();
130
131 return true;
132}

◆ tearDown()

void PositionDirect::tearDown ( )
virtual

Definition at line 134 of file PositionDirect.cpp.

135{
136 if (jointsList) {delete jointsList; jointsList =0;}
137 if (dd) {delete dd; dd =0;}
138}
The documentation for this class was generated from the following files:
Generated by doxygen 1.9.8