PikRos.hpp

Go to the documentation of this file.

#ifndef PIK_ROS_H_
#define PIK_ROS_H_

#include "MoveItWrapper.hpp"

#include <Eigen/Dense>
#include <PtscEigen.hpp>

namespace PikRos
{

enum IkTaskType
{
  FRAME_POSITION, 
  FRAME_ORIENTATION, 
  FRAME_POSE, 
  FRAME_APPROACH_AXIS 
};

enum ApproachAxis
{
  X,
  Y,
  Z
};

struct IkTask
{
  IkTask( IkTaskType t_type, const std::string &t_frame_name, 
          const Eigen::VectorXd &t_desired_value, 
          ApproachAxis t_approach_axis = Z);

  Eigen::Vector3d getAppAxisVector() const;

  IkTaskType type;
  std::string frame_name;
  Eigen::VectorXd desired_value;
  ApproachAxis approach_axis;
};

struct Settings 
{ 
  uint32_t max_iterations = 10000; 
  double  err_norm_threshold = 1e-3; 
  double gradient_threshold = 1e-3; 
  double polish_gradient_threshold = 1e-2; 
  double dq_limit = (double)(10.0 * M_PI / 180.0); 
  double polish_dq_limit = (double)(3.0 * M_PI / 180.0); 
  double lin_err_clamp_magnitude = 0.2; 
  double ang_err_clamp_magnitude = 15.0 * M_PI / 180.0; 
  bool use_constrained_opt = true;
  bool polish_solution = false; 
  bool debug_mode = false; 
  double timeout = 2.0; 
};

class Gradient
{
public:
  Gradient(double initial_value);
  void updateGradient(double current_value);
  double getGradient() const;
private:
  double current_value_;
  double ex_value_;
  double gradient_;
};

class Pik
{
public:
  Pik(ros::NodeHandle &nh_, const std::string &robot_name, 
      const std::string &joint_model_group_name,
      const Settings &settings = Settings());

  Eigen::VectorXd solve(const std::vector<IkTask> &ik_tasks, 
                        const Eigen::VectorXd& q_initial_guess );

  void goToJointPosition(const Eigen::VectorXd &q) const;

  void printErrors(const Eigen::VectorXd &q, const std::vector<IkTask> &ik_tasks) const;

private:
  MoveItWrapper moveit_wrapper_;
  Settings settings_;
  const uint32_t n_dof_;

  Eigen::Vector3d getOrientationErrorRPY( Eigen::Quaterniond desired_quat, 
                                          Eigen::Quaterniond current_quat,
                                          bool clamp_err = false ) const;
          
  bool reachedErrNormThreshold(const Eigen::VectorXd &q, const std::vector<IkTask> &ik_tasks) const;
  static double getAbsGradientSum(const std::vector<Gradient> &gradients);
  bool reachedGradientThreshold(const std::vector<Gradient> &gradients) const;
  
  Eigen::MatrixXd getTaskJacobian( const Eigen::VectorXd &q, IkTask task ) const; 
  Eigen::VectorXd solvePtscProblem(const std::vector<PtscEigen::Task> &ptsc_tasks) const;

  Eigen::VectorXd getTaskValue( const Eigen::VectorXd &q, IkTask task ) const; //These functions go to IkTask!
  Eigen::VectorXd getTaskError( const Eigen::VectorXd &q, IkTask task, bool clamp_err = false ) const; 

  static Eigen::VectorXd clampMagnitude(const Eigen::VectorXd &input_vec, double magnitude);
  static Eigen::VectorXd limitQ(const Eigen::VectorXd &q);
  static Eigen::VectorXd limitDq(const Eigen::VectorXd &dq, double dq_limit);
  static Eigen::Quaterniond vec2quat(const Eigen::VectorXd &q_vec);

  bool reached_polish_solution_threshold_ = false;

  static int sign(double a);

  bool reachedStoppingCriteria() const; 
  void initializeSolver(const std::vector<IkTask> &ik_tasks,
                        const Eigen::VectorXd& q_initial_guess); 

  Eigen::VectorXd current_q_;
  std::vector<PikRos::IkTask> ik_tasks_;
  double solver_elapsed_time_; //in seconds
  std::vector<Gradient> current_gradients_;
  uint32_t current_ik_iteration_;
};
};
#endif //PIK_ROS_H_