26{
27 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
28 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("port"), "The port name must be given as the test parameter!");
29 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("remoteControlBoards"), "Please, provide the controlboards name.");
30 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("axesNames"), "Please, provide the controlled joints name.");
31 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("model"), "Please, provide the urdf model path.");
32 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("maxError"), "Please, provide the threshold error.");
33 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("sensorsList"), "Please, provide the list of the sensors you want to check or 'all' if you want to test all the IMUs.");
34
35 robotName = property.find("robot").asString();
36 portName = property.find("port").asString();
37 errorMax = property.find("maxError").asFloat64();
38 modelName = property.find("model").asString();
39 yarp::os::ResourceFinder &rf = yarp::os::ResourceFinder::getResourceFinderSingleton();
40 std::string modelAbsolutePath = rf.findFileByName(modelName);
41
42 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Running IMU test on "+robotName+"...");
43
44 yarp::os::Property controlBoardOptions;
45 controlBoardOptions.put("device", "remotecontrolboardremapper");
46
47 yarp::os::Bottle remoteControlBoards;
48 yarp::os::Bottle & remoteControlBoardsList = remoteControlBoards.addList();
49 yarp::os::Bottle *inputControlBoards;
50 inputControlBoards = property.find("remoteControlBoards").asList();
51 for(int ctrlBoard = 0; ctrlBoard < inputControlBoards->size(); ctrlBoard++)
52 {
53 partsList.push_back(inputControlBoards->get(ctrlBoard).asString());
54 remoteControlBoardsList.addString("/"+robotName+"/"+inputControlBoards->get(ctrlBoard).asString());
55 }
56
57 yarp::os::Bottle axesNames;
58 yarp::os::Bottle & axesList = axesNames.addList();
59 yarp::os::Bottle *inputJoints;
60 inputJoints = property.find("axesNames").asList();
61 for(int ctrlJoint = 0; ctrlJoint < inputJoints->size(); ctrlJoint++)
62 {
63 axesList.addString(inputJoints->get(ctrlJoint).asString());
64 }
65
66 controlBoardOptions.put("remoteControlBoards", remoteControlBoards.get(0));
67 controlBoardOptions.put("localPortPrefix", "/test");
68 controlBoardOptions.put("axesNames", axesNames.get(0));
69
70 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.open(controlBoardOptions), "Unable to open the controlBoard driver");
71 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.isValid(), "Device driver cannot be opened");
72 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.view(ipos), "Unable to open position control interface");
73 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.view(ienc), "Unable to open encoder interface");
74 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(controlBoardDriver.view(iaxes), "Unable to open axes interface");
75
76 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getAxes(&axes), "Cannot get number of controlled axes");
77 std::string axisName;
78
79 for (int axIndex = 0; axIndex < axes; axIndex++)
80 {
81 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(iaxes->getAxisName(axIndex, axisName), "Cannot get the name of controlled axes");
82 axesVec.push_back(axisName);
83 }
84
85 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(model.loadReducedModelFromFile(modelAbsolutePath.c_str(), axesVec), Asserter::format("Cannot load model from %s", modelAbsolutePath.c_str()));
86 kinDynComp.loadRobotModel(model.model());
87
88 yarp::os::Property MASclientOptions;
89 MASclientOptions.put("device", "multipleanalogsensorsclient");
90 MASclientOptions.put("remote", portName);
91 MASclientOptions.put("local", "/imuTest"+portName);
92
93 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.open(MASclientOptions), "Unable to open the MAS client driver");
94 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.isValid(), "Device driver cannot be opened");
95
96 yarp::os::Bottle & sensorsNamesList = sensorsList.addList();
97 yarp::os::Bottle *inputSensorsList;
98 inputSensorsList = property.find("sensorsList").asList();
99
100 if(inputSensorsList->size() == 0 || inputSensorsList->get(0).asString() == "all")
101 {
102 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Testing all the IMUs available...");
103 std::string sensorName{""};
104 yarp::dev::IOrientationSensors* ior;
105 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASclientDriver.view(ior), "Unable to open the orientation interface");
106 for(int sensorIndex = 0; sensorIndex < ior->getNrOfOrientationSensors(); sensorIndex++)
107 {
108 ior->getOrientationSensorName(sensorIndex, sensorName);
109 sensorsNamesList.addString(sensorName);
110 }
111 }
112
113 else
114 {
115 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Testing the list of provided IMUs...");
116 for(int sensorIndex = 0; sensorIndex < inputSensorsList->size(); sensorIndex++)
117 {
118 sensorsNamesList.addString(inputSensorsList->get(sensorIndex).asString());
119 }
120 }
121
122 yarp::os::Property MASremapperOptions;
123 MASremapperOptions.put("device", "multipleanalogsensorsremapper");
124 MASremapperOptions.put("OrientationSensorsNames", sensorsList.get(0));
125 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASremapperDriver.open(MASremapperOptions), "Unable to open the MAS remapper driver");
126 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASremapperDriver.isValid(), "Device driver cannot be opened");
127
128 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASremapperDriver.view(imultiwrap), "Unable to open multiple wrapper interface");
129
130 yarp::dev::PolyDriverList driverList;
131 driverList.push(&MASclientDriver, "alljoints_inertials");
132 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(imultiwrap->attachAll(driverList), "Unable to do the attach");
133 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(MASremapperDriver.view(iorientation), "Unable to open orientation interface");
134
135 iDynTree::Vector3 baseLinkOrientationRad;
136 baseLinkOrientationRad.zero();
137
138 baseLinkOrientation = iDynTree::Rotation::RPY(baseLinkOrientationRad[0], baseLinkOrientationRad[1], baseLinkOrientationRad[2]);
139 I_T_base = iDynTree::Transform(baseLinkOrientation, iDynTree::Position::Zero());
140 iDynTree::Twist baseVelocity = iDynTree::Twist::Zero();
141
142 unsigned int dofs = kinDynComp.model().getNrOfDOFs();
143 s.resize(dofs);
144 ds.resize(dofs);
145
146 positions.resize(axes);
147 velocities.resize(axes);
148 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getEncoders(positions.data()), "Cannot get joint positions");
149 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ienc->getEncoderSpeeds(velocities.data()), "Cannot get joint velocities");
150
151 for(auto axIndex = 0; axIndex < axes; axIndex++)
152 {
153 s.setVal(axIndex, iDynTree::deg2rad(positions[axIndex]));
154 ds.setVal(axIndex, iDynTree::deg2rad(velocities[axIndex]));
155 }
156
157 gravity.zero();
158 gravity(2) = -9.81;
159
160 kinDynComp.setRobotState(
161 I_T_base,
162 s,
163 baseVelocity,
164 ds,
165 gravity
166 );
167
168 setupRobometry();
169 localBroker.resize(remoteControlBoards.get(0).asList()->size());
170
171 return true;
172}
