3 #include <visp3/core/vpConfig.h>
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>
11 #include <nlohmann/json.hpp>
12 using json = nlohmann::json;
15 enum vpInteractionMatrixTypeSubset
24 NLOHMANN_JSON_SERIALIZE_ENUM(vpInteractionMatrixTypeSubset, {
70 void from_json(
const json &j,
Arguments &a)
77 a.
cMo = j.value(
"cMo", a.
cMo);
110 Arguments readArguments(
const std::string &path)
115 std::ifstream file(path);
117 std::stringstream ss;
118 ss <<
"Problem opening file " << path <<
". Make sure it exists and is readable" << std::endl;
123 j = json::parse(file);
125 catch (json::parse_error &e) {
126 std::stringstream msg;
127 msg <<
"Could not parse JSON file : \n";
129 msg << e.what() << std::endl;
130 msg <<
"Byte position of error: " << e.byte;
137 std::cout <<
"Using default arguments. Try using a JSON file to set the arguments of the visual servoing." << std::endl;
160 m_arguments(arguments), m_desiredFeatures(desiredFeatures)
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);
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;
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}
202 std::ofstream file(path);
203 const json j = results;
209 int main(
int argc,
char *argv[])
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]);
218 else if (std::string(argv[i]) ==
"--output" && i + 1 < argc) {
219 output_path = std::string(argv[i + 1]);
223 if (output_path.empty()) {
224 std::cerr <<
"JSON output path must be specified" << std::endl;
227 const Arguments args = readArguments(arguments_path);
233 std::cout << cdMo << std::endl;
247 std::vector<vpFeaturePoint> features;
249 for (
unsigned int i = 0; i < 4; i++) {
250 point[i].
track(cdMo);
264 robot.getPosition(wMc);
267 unsigned int iter = 0;
269 std::cout <<
"Start visual-servoing loop until convergence..." << std::endl;
272 robot.getPosition(wMc);
274 for (
unsigned int i = 0; i < 4; i++) {
294 std::cout <<
"Convergence in " << iter <<
" iterations" << std::endl;
296 saveResults(results, output_path);
300 std::cout <<
"Caught an exception: " << e << std::endl;
306 std::cerr <<
"Cannot run tutorial: ViSP is not built with JSON integration. Install the JSON library and recompile ViSP" << std::endl;
vpInteractionMatrixTypeSubset interactionMatrixType
vpServo::vpServoIteractionMatrixType getInteractionMatrixType() const
[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.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
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...
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...
Implementation of a matrix and operations on matrices.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void setWorldCoordinates(double oX, double oY, double oZ)
Implementation of a pose vector and operations on poses.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
vpMatrix getInteractionMatrix() const
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
vpServoIteractionMatrixType
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that defines the simplest robot: a free flying camera.
VISP_EXPORT int wait(double t0, double t)