Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
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>
12 using json = nlohmann::json;
13 
15 enum vpInteractionMatrixTypeSubset
16 {
17  UNKNOWN = -1,
18  CURRENT,
19  DESIRED,
20  MEAN
21 };
24 NLOHMANN_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 
33 class Arguments
34 {
35 public:
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)),
40  samplingTime(0.04), errorThreshold(0.0001), interactionMatrixType(CURRENT)
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
70 void 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 
96 void 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 
110 Arguments 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 
144 void 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 {
158 public:
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 
174 private:
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 
185 void 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 
200 void 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 
209 int 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 << "Starting 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
304 int 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.
Definition: vpColVector.h:163
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:109
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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.
Definition: vpPoseVector.h:189
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ CAMERA_FRAME
Definition: vpRobot.h:82
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:519
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:378
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:329
void setLambda(double c)
Definition: vpServo.h:976
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:132
vpColVector getError() const
Definition: vpServo.h:504
vpColVector computeControlLaw()
Definition: vpServo.cpp:703
vpServoIteractionMatrixType
Definition: vpServo.h:190
@ DESIRED
Definition: vpServo.h:202
@ MEAN
Definition: vpServo.h:208
@ CURRENT
Definition: vpServo.h:196
Class that defines the simplest robot: a free flying camera.
VISP_EXPORT int wait(double t0, double t)