Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-ibvs-4pts-json.cpp
1
2#include <iostream>
3#include <visp3/core/vpConfig.h>
4
5#ifdef VISP_HAVE_NLOHMANN_JSON
6#include <visp3/robot/vpSimulatorCamera.h>
7#include <visp3/visual_features/vpFeatureBuilder.h>
8#include <visp3/vs/vpServo.h>
9
10
11#include <nlohmann/json.hpp>
12using json = nlohmann::json;
13
15enum vpInteractionMatrixTypeSubset
16{
17 UNKNOWN = -1,
18 CURRENT,
19 DESIRED,
20 MEAN
21};
24NLOHMANN_JSON_SERIALIZE_ENUM(vpInteractionMatrixTypeSubset, {
25 {UNKNOWN, nullptr}, // Default value if the json string is not in "current", "desired" or "mean"
26 {CURRENT, "current"},
27 {DESIRED, "desired"},
28 {MEAN, "mean"} }
29);
31
34{
35public:
36 // Default values
38 lambda(0.5), cdMo(0, 0, 0.75, 0, 0, 0),
39 cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50)),
41 { }
42
44 {
45 switch (interactionMatrixType) {
46 case CURRENT:
47 return vpServo::CURRENT;
48 case DESIRED:
49 return vpServo::DESIRED;
50 case MEAN:
51 return vpServo::MEAN;
52 default:
53 throw vpException(vpException::badValue, "Unexpected value");
54 }
55 return vpServo::CURRENT;
56 }
57
58 double lambda; // Control law gain
59 vpHomogeneousMatrix cdMo; // Target (desired) camera pose
60 vpHomogeneousMatrix cMo; // Initial camera pose
61 double samplingTime; // Robot sampling time
62 double errorThreshold; // Error threshold. Once error is below, consider servoing as successful
63 vpInteractionMatrixTypeSubset interactionMatrixType;
64};
66
68// Read script arguments from JSON. All values are optional and if an argument is not present,
69// the default value defined in the constructor is kept
70void from_json(const json &j, Arguments &a)
71{
72 a.lambda = j.value("lambda", a.lambda);
73 if (a.lambda <= 0) {
74 throw vpException(vpException::badValue, "Lambda should be > 0");
75 }
76
77 a.cMo = j.value("cMo", a.cMo);
78 a.cdMo = j.value("cdMo", a.cdMo);
79
80 a.samplingTime = j.value("samplingTime", a.samplingTime);
81 if (a.samplingTime <= 0) {
82 throw vpException(vpException::badValue, "Sampling time should be > 0");
83 }
84
85 a.errorThreshold = j.value("errorThreshold", a.errorThreshold);
86 if (a.errorThreshold <= 0) {
87 throw vpException(vpException::badValue, "Error threshold should be > 0");
88 }
89
90 a.interactionMatrixType = j.value("interactionMatrix", a.interactionMatrixType);
91 if (a.interactionMatrixType == UNKNOWN) {
92 throw vpException(vpException::badValue, "Unknown interaction matrix type defined in JSON");
93 }
94}
95
96void to_json(json &j, const Arguments &a)
97{
98 j = json {
99 {"lambda", a.lambda},
100 {"cMo", a.cMo},
101 {"cdMo", a.cdMo},
102 {"errorThreshold", a.errorThreshold},
103 {"samplingTime", a.samplingTime},
104 {"interactionMatrix", a.interactionMatrixType}
105 };
106}
108
110Arguments readArguments(const std::string &path)
111{
112 Arguments a;
113
114 if (!path.empty()) {
115 std::ifstream file(path);
116 if (!file.good()) {
117 std::stringstream ss;
118 ss << "Problem opening file " << path << ". Make sure it exists and is readable" << std::endl;
119 throw vpException(vpException::badValue, ss.str());
120 }
121 json j;
122 try {
123 j = json::parse(file);
124 }
125 catch (json::parse_error &e) {
126 std::stringstream msg;
127 msg << "Could not parse JSON file : \n";
128
129 msg << e.what() << std::endl;
130 msg << "Byte position of error: " << e.byte;
131 throw vpException(vpException::ioError, msg.str());
132 }
133 a = j; // Call from_json(const json& j, Argument& a) to read json into arguments a
134 file.close();
135 }
136 else {
137 std::cout << "Using default arguments. Try using a JSON file to set the arguments of the visual servoing." << std::endl;
138 }
139 return a;
140}
142
144void to_json(json &j, const vpFeaturePoint &p)
145{
146 j = json {
147 {"x", p.get_x()},
148 {"y", p.get_y()},
149 {"z", p.get_Z()}
150 };
151}
152
154
157{
158public:
159 ServoingExperimentData(const Arguments &arguments, const std::vector<vpFeaturePoint> &desiredFeatures) :
160 m_arguments(arguments), m_desiredFeatures(desiredFeatures)
161 { }
162
163 void onIter(const vpHomogeneousMatrix &cMo, const double errorNorm, const std::vector<vpFeaturePoint> &points,
164 const vpColVector &velocity, const vpMatrix &interactionMatrix)
165 {
166 vpPoseVector r(cMo);
167 m_trajectory.push_back(r);
168 m_errorNorms.push_back(errorNorm);
169 m_points3D.push_back(points);
170 m_velocities.push_back(velocity);
171 m_interactionMatrices.push_back(interactionMatrix);
172 }
173
174private:
175 Arguments m_arguments;
176 std::vector<vpFeaturePoint> m_desiredFeatures;
177 std::vector<vpPoseVector> m_trajectory;
178 std::vector<double> m_errorNorms;
179 std::vector<std::vector<vpFeaturePoint> > m_points3D;
180 std::vector<vpColVector> m_velocities;
181 std::vector<vpMatrix> m_interactionMatrices;
182 friend void to_json(json &j, const ServoingExperimentData &res);
183};
184
185void to_json(json &j, const ServoingExperimentData &res)
186{
187 j = json {
188 {"parameters", res.m_arguments},
189 {"trajectory", res.m_trajectory},
190 {"errorNorm", res.m_errorNorms},
191 {"features", res.m_points3D},
192 {"desiredFeatures", res.m_desiredFeatures},
193 {"velocities", res.m_velocities},
194 {"interactionMatrices", res.m_interactionMatrices}
195 };
196}
198
200void saveResults(const ServoingExperimentData &results, const std::string &path)
201{
202 std::ofstream file(path);
203 const json j = results;
204 file << j.dump(4);
205 file.close();
206}
208
209int main(int argc, char *argv[])
210{
212 std::string arguments_path = "";
213 std::string output_path = "";
214 for (int i = 1; i < argc; ++i) {
215 if (std::string(argv[i]) == "--settings" && i + 1 < argc) {
216 arguments_path = std::string(argv[i + 1]);
217 }
218 else if (std::string(argv[i]) == "--output" && i + 1 < argc) {
219 output_path = std::string(argv[i + 1]);
220 }
221 }
222
223 if (output_path.empty()) {
224 std::cerr << "JSON output path must be specified" << std::endl;
225 return EXIT_FAILURE;
226 }
227 const Arguments args = readArguments(arguments_path);
229
230 try {
231 vpHomogeneousMatrix cdMo = args.cdMo;
232 vpHomogeneousMatrix cMo = args.cMo;
233 std::cout << cdMo << std::endl;
234
235 vpPoint point[4];
236 point[0].setWorldCoordinates(-0.1, -0.1, 0);
237 point[1].setWorldCoordinates(0.1, -0.1, 0);
238 point[2].setWorldCoordinates(0.1, 0.1, 0);
239 point[3].setWorldCoordinates(-0.1, 0.1, 0);
240
241 vpServo task;
244 task.setLambda(args.lambda);
245
246 vpFeaturePoint p[4], pd[4];
247 std::vector<vpFeaturePoint> features;
248 features.resize(4);
249 for (unsigned int i = 0; i < 4; i++) {
250 point[i].track(cdMo);
251 vpFeatureBuilder::create(pd[i], point[i]);
252 point[i].track(cMo);
253 vpFeatureBuilder::create(p[i], point[i]);
254 task.addFeature(p[i], pd[i]);
255 features[i] = pd[i];
256 }
258 ServoingExperimentData results(args, features);
260
261 vpHomogeneousMatrix wMc, wMo;
262 vpSimulatorCamera robot;
263 robot.setSamplingTime(args.samplingTime);
264 robot.getPosition(wMc);
265 wMo = wMc * cMo;
266
267 unsigned int iter = 0;
268 bool end = false;
269 std::cout << "Start visual-servoing loop until convergence..." << std::endl;
271 while (!end) {
272 robot.getPosition(wMc);
273 cMo = wMc.inverse() * wMo;
274 for (unsigned int i = 0; i < 4; i++) {
275 point[i].track(cMo);
276 vpFeatureBuilder::create(p[i], point[i]);
277 features[i] = p[i];
278 }
279 const vpColVector v = task.computeControlLaw();
281 const double errorNorm = task.getError().sumSquare();
282
284 results.onIter(cMo, errorNorm, features, v, task.getInteractionMatrix());
286
287 if (errorNorm < args.errorThreshold) {
288 end = true;
289 }
290 vpTime::wait(10);
291 iter++;
292 }
294 std::cout << "Convergence in " << iter << " iterations" << std::endl;
296 saveResults(results, output_path);
298 }
299 catch (const vpException &e) {
300 std::cout << "Caught an exception: " << e << std::endl;
301 }
302}
303#else
304int main()
305{
306 std::cerr << "Cannot run tutorial: ViSP is not built with JSON integration. Install the JSON library and recompile ViSP" << std::endl;
307}
308#endif
[Enum conversion]
vpInteractionMatrixTypeSubset interactionMatrixType
vpServo::vpServoIteractionMatrixType getInteractionMatrixType() const
vpHomogeneousMatrix cdMo
vpHomogeneousMatrix cMo
[Custom ViSP object conversion]
friend void to_json(json &j, const ServoingExperimentData &res)
void onIter(const vpHomogeneousMatrix &cMo, const double errorNorm, const std::vector< vpFeaturePoint > &points, const vpColVector &velocity, const vpMatrix &interactionMatrix)
ServoingExperimentData(const Arguments &arguments, const std::vector< vpFeaturePoint > &desiredFeatures)
Implementation of column vector and the associated operations.
double sumSquare() const
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
double get_y() const
double get_x() const
double get_Z() const
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Provides simple mathematics computation tools that are not available in the C mathematics library (ma...
Definition vpMath.h:98
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
Implementation of a pose vector and operations on poses.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition vpRobot.h:80
vpMatrix getInteractionMatrix() const
Definition vpServo.h:291
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_CAMERA
Definition vpServo.h:151
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
vpServoIteractionMatrixType
Definition vpServo.h:178
@ DESIRED
Definition vpServo.h:183
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.
VISP_EXPORT int wait(double t0, double t)