visual_servoing.h

Go to the documentation of this file.

#ifndef VISUAL_SERVOING_H
#define VISUAL_SERVOING_H

#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/SVD>

#include "visual_servoing/robot_controller.h"
#include "visual_servoing/tracker_manager.h"
#include "visual_servoing/Matrix.h"

// Image Based Visual Servoing

class IBVS {
  public:
      bool broyden_updates;
      double learn_rate;

      IBVS(RobotController* rc, TrackerManager* tm);

      Eigen::VectorXd step(double lambda=1);
      Eigen::VectorXd do_step(double lambda=1);

      void broyden_update();

      void update_jacobian(double delta, size_t num_joints);

      void converge(double timeout, double lambda);
      void step_converge(int steps, double lambda);

  private:
      // Broyden update parameters
      int broyden_thr;
      Eigen::VectorXd prev_error;
      Eigen::VectorXd prev_jnt_state;
      Eigen::VectorXd prev_eef_pos;

      RobotController* robot;
      TrackerManager* trackers;

      Eigen::MatrixXd jacobian;
      Eigen::MatrixXd jacobian_inv;
      ros::Publisher jacobian_pub;

      void publish_jacobian();
      float get_cond(Eigen::MatrixXd& m);
};

#endif // VISUAL_SERVOING_H