text:
#include<iostream>
#include <string>
#include <ros/ros.h>
#include <ros/time.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>
#include <robot_state_publisher/robot_state_publisher.h>
#include <cmath>
#include <math.h>
#include <eigen3/Eigen/Dense>
#include <Eigen/Dense>
#include "gazebo_msgs/LinkStates.h"
#include "gazebo_msgs/LinkState.h"

#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"

#include "inverse_kinematics.h"

using namespace std;
Eigen::Matrix<double, 3, 1> q_i_LF;
Eigen::Matrix<double, 3, 1> q_i_LH;
Eigen::Matrix<double, 3, 1> q_i_RF;
Eigen::Matrix<double, 3, 1> q_i_RH;

Eigen::Matrix<double, 3, 1> pos_foot_LF;
Eigen::Matrix<double, 3, 1> pos_foot_LH;
Eigen::Matrix<double, 3, 1> pos_foot_RF;
Eigen::Matrix<double, 3, 1> pos_foot_RH;

Eigen::Matrix<double, 6, 1> pos_base;

bool friction_cone;
bool friction_cone_dietro;
bool friction_dietro;
bool friction_dietro_due;
bool scivolo;
bool scivolo_dietro;
bool scivolo_due;
bool scivolo_dietro_due;

bool fine;
bool endd;
bool freeze;
double delta_x_avanti;
double conta_friction;

void get_scivolo(const std_msgs::BoolConstPtr& msg)
{
 scivolo = msg->data;
}

void get_delta_x_avanti(const std_msgs::Float64ConstPtr& msg)
{
 delta_x_avanti = msg->data;
}

void get_conta_friction(const std_msgs::Float64ConstPtr& msg)
{
 conta_friction= msg->data;
}

void get_scivolo_dietro(const std_msgs::BoolConstPtr& msg)
{
 scivolo_dietro = msg->data;
}

void get_scivolo_due(const std_msgs::BoolConstPtr& msg)
{
 scivolo_due = msg->data;
}

void get_scivolo_dietro_due(const std_msgs::BoolConstPtr& msg)
{
 scivolo_dietro_due = msg->data;
}

void get_freeze(const std_msgs::BoolConstPtr& msg)
{
 freeze = msg->data;
}

void get_friction_cone(const std_msgs::BoolConstPtr& msg)
{
 friction_cone = msg->data;
}

void get_friction_cone_dietro(const std_msgs::BoolConstPtr& msg)
{
 friction_cone_dietro = msg->data;
}

void get_friction_dietro(const std_msgs::BoolConstPtr& msg)
{
 friction_dietro = msg->data;
}

void get_friction_dietro_due(const std_msgs::BoolConstPtr& msg)
{
 friction_dietro_due = msg->data;
}

void get_pos_base(const geometry_msgs::TwistConstPtr& msg)
{
        pos_base(0) = msg->linear.x;
        pos_base(1) = msg->linear.y;
        pos_base(2) = msg->linear.z;

        pos_base(3)= msg->angular.x;
        pos_base(4)= msg->angular.y;
        pos_base(5)= msg->angular.z;

}

void get_q_i_L(const geometry_msgs::TwistConstPtr& msg)
{
        q_i_LF(0) = msg->linear.x;
        q_i_LF(1) = msg->linear.y;
        q_i_LF(2) = msg->linear.z;

        q_i_LH(0) = msg->angular.x;
        q_i_LH(1) = msg->angular.y;
        q_i_LH(2) = msg->angular.z;

}

void get_q_i_R(const geometry_msgs::TwistConstPtr& msg)
{
        q_i_RF(0) = msg->linear.x;
        q_i_RF(1) = msg->linear.y;
        q_i_RF(2) = msg->linear.z;

        q_i_RH(0) = msg->angular.x;
        q_i_RH(1) = msg->angular.y;
        q_i_RH(2) = msg->angular.z;

}

void get_L_foot_pos(const geometry_msgs::TwistConstPtr& msg)
{
      pos_foot_LF(0) = msg->linear.x;
      pos_foot_LF(1) = msg->linear.y;
      pos_foot_LF(2) = msg->linear.z;

      pos_foot_LH(0) = msg->angular.x;
      pos_foot_LH(1) = msg->angular.y;
      pos_foot_LH(2) = msg->angular.z;
}

void get_R_foot_pos(const geometry_msgs::TwistConstPtr& msg)
{
      pos_foot_RF(0) = msg->linear.x;
      pos_foot_RF(1) = msg->linear.y;
      pos_foot_RF(2) = msg->linear.z;

      pos_foot_RH(0) = msg->angular.x;
      pos_foot_RH(1) = msg->angular.y;
      pos_foot_RH(2) = msg->angular.z;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "set_command"); //The name of the node
  ros::NodeHandle n;
  ros::Rate loop_rate(100);

  ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("/anymal/floating_base_controller/command", 1);
  ros::Publisher base_pub = n.advertise<sensor_msgs::JointState>("/anymal/floating_base_controller/command_base", 1);

  ros::Subscriber q_i_L_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/q_i_L_pub", 1, get_q_i_L);
  ros::Subscriber q_i_R_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/q_i_R_pub", 1, get_q_i_R);

  ros::Subscriber pos_foot_L_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/pos_foot_L_pub", 1, get_L_foot_pos);
  ros::Subscriber pos_foot_R_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/pos_foot_R_pub", 1, get_R_foot_pos);

  ros::Subscriber pos_base_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/pos_base_pub", 1, get_pos_base);

  ros::Subscriber freeze_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/freeze_pub", 1, get_freeze);
  ros::Subscriber friction_cone_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_cone_pub", 1, get_friction_cone);
  ros::Subscriber friction_cone_dietro_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_cone_dietro_pub", 1, get_friction_cone_dietro);
  ros::Subscriber friction_dietro_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_dietro_pub", 1, get_friction_dietro);
  ros::Subscriber friction_dietro_due_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_dietro_due_pub", 1, get_friction_dietro_due);
  ros::Subscriber scivolo_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_pub", 1, get_scivolo);
  ros::Subscriber scivolo_due_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_due_pub", 1, get_scivolo_due);
  ros::Subscriber scivolo_dietro_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_dietro_pub", 1, get_scivolo);
  ros::Subscriber scivolo_dietro_due_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_dietro_due_pub", 1, get_scivolo_dietro_due);
  ros::Subscriber delta_x_avanti_sub = n.subscribe<std_msgs::Float64>("/anymal/floating_base_controller/delta_x_avanti_pub", 1000, get_delta_x_avanti);
  ros::Subscriber conta_friction_sub = n.subscribe<std_msgs::Float64>("/anymal/floating_base_controller/conta_friction_pub", 1000, get_conta_friction);

  ros::Publisher c_switch_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_c_switch_set_command", 1);
  ros::Publisher fine_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_fine_set_command", 1);
  ros::Publisher end_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_end_set_command", 1);
  ros::Publisher scivolo_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo", 1);
  ros::Publisher scivolo_due_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo_due", 1);
  ros::Publisher scivolo_dietro_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo_dietro", 1);
  ros::Publisher scivolo_dietro_due_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo_dietro_due", 1);
  ros::Publisher conta_friction_pub = n.advertise<std_msgs::Float64>("/anymal/floating_base_controller/conta_friction_pub", 1000);
  ros::Publisher delta_x_avanti_pub = n.advertise<std_msgs::Float64>("/anymal/floating_base_controller/delta_x_avanti_pub", 1000);

  sensor_msgs::JointState joint_state;
  sensor_msgs::JointState base_state;

  double q1_time_LF; //= q_i_LF(0);
  double q2_time_LF; //= q_i_LF(1);
  double q3_time_LF; //= q_i_LF(2);

  double q1_f_LF;
  double q2_f_LF;
  double q3_f_LF;

  double q1_time_LH;
  double q2_time_LH;
  double q3_time_LH;

  double q1_f_LH;
  double q2_f_LH;
  double q3_f_LH;

  double q1_time_RF;
  double q2_time_RF;
  double q3_time_RF;

  double q1_f_RF;
  double q2_f_RF;
  double q3_f_RF;

  double q1_time_RH;
  double q2_time_RH;
  double q3_time_RH;

  double q1_f_RH;
  double q2_f_RH;
  double q3_f_RH;

double delta_x_wp1_L = 0.085;
double delta_x_wp2_L = 0.085;
double delta_x_wp1_R = 0.085;
double delta_x_wp2_R = 0.085;

  double delta_x_wp1_Ll = 0.074;
  double delta_y_wp1_L = 0.0;
  double delta_h_wp1_L = 0.025;

  double delta_x_wp2_Ll = 0.074;
  double delta_y_wp2_L = 0.0;
  double delta_h_wp2_L = -0.025;

  double delta_x_wp1_LF = 0.0742;
  double delta_y_wp1_LF = 0.0;
  double delta_h_wp1_LF = 0.025;

  double delta_x_wp2_LF = 0.0742;
  double delta_y_wp2_LF = 0.0;
  double delta_h_wp2_LF = -0.025;

  double delta_x_wp1_LH = 0.0755; //0.0755;
  double delta_y_wp1_LH = 0.0;
  double delta_h_wp1_LH = 0.025;

  double delta_x_wp2_LH = 0.0755; //0.0755;
  double delta_y_wp2_LH = 0.0;
  double delta_h_wp2_LH = -0.025;

  double delta_x_wp1_RF = 0.076;
  double delta_y_wp1_RF = 0.0;
  double delta_h_wp1_RF = 0.025;

  double delta_x_wp2_RF = 0.076;
  double delta_y_wp2_RF = 0.0;
  double delta_h_wp2_RF = -0.025;

  double delta_x_wp1_RH = 0.08;
  double delta_y_wp1_RH = 0.0;
  double delta_h_wp1_RH = 0.025;

  double delta_x_wp2_RH = 0.08;
  double delta_y_wp2_RH = 0.0;
  double delta_h_wp2_RH = -0.025;

  double delta_y_wp1_RF_fd = -0.004;
  double delta_y_wp2_RF_fd = -0.004;

   double delta_x_wp1_RFF = 0.08;//0.04; //0.035;
   double delta_y_wp1_RFF = -0.0025;//2;//0.006;
   double delta_h_wp1_RFF = 0.025;

   double delta_x_wp2_RFF = 0.08; //0.04; // 0.035
   double delta_y_wp2_RFF = -0.0025; //2;//0.006;
   double delta_h_wp2_RFF = -0.025;

   double delta_x_wp1_RHH = 0.076;
   double delta_y_wp1_RHH = -0.008; //-0.02;
   double delta_h_wp1_RHH = 0.025;

   double delta_x_wp2_RHH = 0.076;
   double delta_y_wp2_RHH = -0.008; //-0.02;
   double delta_h_wp2_RHH = -0.025;

   double delta_x_wp1_LFF = 0.075;
   double delta_y_wp1_LFF = 0.0025; //0.008;//0.008;//0.006;//0.02;
   double delta_h_wp1_LFF = 0.025;

   double delta_x_wp2_LFF = 0.075;
   double delta_y_wp2_LFF = 0.0025; //0.008; //0.008; //0.006; //0.02;
   double delta_h_wp2_LFF = -0.025;

   double delta_x_wp1_LHH = 0.075; //0.072; //0.075;
   double delta_y_wp1_LHH = 0.0005;//0.002 -0.004; //-0.008; //-0.004;//-0.003; //0.0;//-0.03; //-0.03;
   double delta_h_wp1_LHH = 0.025;

   double delta_x_wp2_LHH = 0.075; //0.072; //0.075;
   double delta_y_wp2_LHH = 0.0005; //0.002 -0.004; //-0.008; //-0.004; //- 0.003; //0.0; //-0.03; //-0.03;
   double delta_h_wp2_LHH = -0.025;

  double x_wp_1;
  double y_wp_1;
  double z_wp_1;
  double x_wp_2;
  double y_wp_2;
  double z_wp_2;

  double conta = 0;

  double delta_x = 0.078;// - delta_x_avanti;
  double delta_xx = 0.078; // - delta_x_avanti;

  double x_wp_dxx = 0.075;
  double x_wp_dx = 0.09;
  double y_wp_dx = 0.08;
  double z_wp_dx = 0.54;
  double roll_wp_dx = -0.03;
  double pitch_wp_dx = -0.01;
  double yaw_wp_dx = 0.0;

  double x_wp_sxx = 0.045;
  double x_wp_sx = 0.06;
  double y_wp_sx = - 0.08;
  double z_wp_sx = 0.54;
  double roll_wp_sx = 0.02;
  double pitch_wp_sx = -0.01;
  double yaw_wp_sx = 0.0;

  double x_time;
  double y_time;
  double z_time;
  double roll_time;
  double pitch_time;
  double yaw_time;

  double pitch = 0.2;

  double time_f = 0.5;
  double time_f_base = 2;

  double contatore = 0;

  double t;

  bool leg_up = 1;
  bool c_switch = 0;

  bool base_dx = 0;

 std_msgs::Bool c_switch_msg;
 std_msgs::Bool fine_msg;  // c_switch
 std_msgs::Bool end_msg;
 std_msgs::Bool scivolo_msg;

  Eigen::Matrix<double, 4, 1> move_leg;
  move_leg << 0 ,1 , 0, 0;

  while (ros::ok())
    {

      t = ros::Time::now().toSec();

      if(t > 2 && t < 2.1)
      {
        c_switch = 1;
      }

      //if (c_switch == 1 && t < 95)

      if (c_switch == 1 && scivolo == 0 && scivolo_due == 0 && scivolo_dietro == 0 && scivolo_dietro_due == 0)
      {
           if ( move_leg(1) == 1)
        {
          q1_time_LF = q_i_LF(0);
          q2_time_LF = q_i_LF(1);
          q3_time_LF = q_i_LF(2);

          q1_time_LH = q_i_LH(0);
          q2_time_LH = q_i_LH(1);
          q3_time_LH = q_i_LH(2);

          q1_time_RF = q_i_RF(0);
          q2_time_RF = q_i_RF(1);
          q3_time_RF = q_i_RF(2);

          q1_time_RH = q_i_RH(0);
          q2_time_RH = q_i_RH(1);
          q3_time_RH = q_i_RH(2);

          if(friction_cone == 0){
          if(conta == 0 || conta == 1){
            scivolo = 0;
            scivolo_due = 0;
            scivolo_dietro = 0;
            scivolo_dietro_due = 0;

          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_LH(0) + delta_x_wp1_Ll;
            y_wp_1 = pos_foot_LH(1) + delta_y_wp1_L;
            z_wp_1 = pos_foot_LH(2) + delta_h_wp1_L;

            Inverse_Kinematics_LH(x_wp_1,y_wp_1,z_wp_1,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LH(0) = q1_f_LH;
              q_i_LH(1) = q2_f_LH;
              q_i_LH(2) = q3_f_LH;
              contatore = 0;
              leg_up = 0;

            }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_Ll;
            y_wp_2 = y_wp_1 + delta_y_wp2_L;
            z_wp_2 = z_wp_1 + delta_h_wp2_L;

            Inverse_Kinematics_LH(x_wp_2,y_wp_2,z_wp_2,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 1,0,0,0;
              conta++;

            }
          }

        }

        else{

           scivolo = 0;
            scivolo_due = 0;
            scivolo_dietro = 0;
            scivolo_dietro_due = 0;

          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_LH(0) + delta_x_wp1_LH;
            y_wp_1 = pos_foot_LH(1) + delta_y_wp1_LH;
            z_wp_1 = pos_foot_LH(2) + delta_h_wp1_LH;

            Inverse_Kinematics_LH(x_wp_1,y_wp_1,z_wp_1,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LH(0) = q1_f_LH;
              q_i_LH(1) = q2_f_LH;
              q_i_LH(2) = q3_f_LH;
              contatore = 0;
              leg_up = 0;

            }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_LH;
            y_wp_2 = y_wp_1 + delta_y_wp2_LH;
            z_wp_2 = z_wp_1 + delta_h_wp2_LH;

            Inverse_Kinematics_LH(x_wp_2,y_wp_2,z_wp_2,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 1,0,0,0;

            }
          }

        }
          }

        else{
          if( friction_cone == 1){
          if(leg_up == 1)
          {
            friction_cone = 1;
            x_wp_1 = pos_foot_LH(0) + delta_x_wp1_LHH;//delta_x_wp1_LH_fd;
            y_wp_1 = pos_foot_LH(1) + delta_y_wp1_LH;
            z_wp_1 = pos_foot_LH(2) + delta_h_wp1_LHH;

            Inverse_Kinematics_LH(x_wp_1,y_wp_1,z_wp_1,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LH(0) = q1_f_LH;
              q_i_LH(1) = q2_f_LH;
              q_i_LH(2) = q3_f_LH;
              contatore = 0;
              leg_up = 0;
              friction_cone = 1;

            }
          }

          else
          {
            friction_cone = 1;
            x_wp_2 = x_wp_1 + delta_x_wp2_LHH;// delta_x_wp2_LH_fd;
            y_wp_2 = y_wp_1 + delta_y_wp2_LH;
            z_wp_2 = z_wp_1 + delta_h_wp2_LHH;
            //cout<<"x_wp_sx_LL"<<endl<<x_wp_2<<endl;

            Inverse_Kinematics_LH(x_wp_2,y_wp_2,z_wp_2,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 1,0,0,0;
              friction_cone = 1;

           }
          }
         }
        }
        }

        else if ( move_leg(0) == 1)
        {
          if(friction_cone == 0){
          cout<<"normale"<<endl;
          scivolo = 0;
          scivolo_due = 0;
          scivolo_dietro = 0;
          scivolo_dietro_due = 0;
          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_LF(0) + delta_x_wp1_LF;
            y_wp_1 = pos_foot_LF(1) + delta_y_wp1_LF;
            z_wp_1 = pos_foot_LF(2) + delta_h_wp1_LF;

            Inverse_Kinematics_LF(x_wp_1,y_wp_1,z_wp_1,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LF(0) = q1_f_LF;
              q_i_LF(1) = q2_f_LF;
              q_i_LF(2) = q3_f_LF;
              contatore = 0;
              leg_up = 0;

            }

          }

          else{

            x_wp_2 = x_wp_1 + delta_x_wp2_LF;
            y_wp_2 = y_wp_1 + delta_y_wp2_LF;
            z_wp_2 = z_wp_1 + delta_h_wp2_LF;

            Inverse_Kinematics_LF(x_wp_2,y_wp_2,z_wp_2,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
                contatore = 0;
                leg_up = 1;
                move_leg << 0,0,0,1;
              c_switch = 0;
              base_dx = 1;

              x_time = x_wp_sx;
              y_time = y_wp_sx;
              z_time = z_wp_sx;
              roll_time = roll_wp_sx;
              pitch_time = pitch_wp_sx;
              yaw_time = yaw_wp_sx;

              delta_x_wp1_L = delta_x_wp1_R;
              delta_x_wp2_L = delta_x_wp2_R;

            }

          }
        }
        else{
          if(leg_up == 1)
          {
            friction_cone = 1;

            x_wp_1 = pos_foot_LF(0) + delta_x_wp1_LF;
            y_wp_1 = pos_foot_LF(1) + delta_y_wp1_LFF;
            z_wp_1 = pos_foot_LF(2) + delta_h_wp1_LFF;

            Inverse_Kinematics_LF(x_wp_1,y_wp_1,z_wp_1,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LF(0) = q1_f_LF;
              q_i_LF(1) = q2_f_LF;
              q_i_LF(2) = q3_f_LF;
              contatore = 0;
              leg_up = 0;
              friction_cone = 1;

            }

          }

          else{

            friction_cone = 1;
            x_wp_2 = x_wp_1 + delta_x_wp2_LF;
            y_wp_2 = y_wp_1 + delta_y_wp2_LFF;
            z_wp_2 = z_wp_1 + delta_h_wp2_LFF;

            Inverse_Kinematics_LF(x_wp_2,y_wp_2,z_wp_2,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
                contatore = 0;
                leg_up = 1;
                move_leg << 0,0,0,1;
                c_switch = 0;
                base_dx = 1;
                friction_cone = 1;

                x_time = x_wp_sx;
                y_time = y_wp_sx;
                z_time = z_wp_sx;
                roll_time = roll_wp_sx;
                pitch_time = pitch_wp_sx;
                yaw_time = yaw_wp_sx;

                delta_x_wp1_L = delta_x_wp1_R;
                delta_x_wp2_L = delta_x_wp2_R;

            }

          }
        }

        }

     else if(move_leg(3) == 1)
        {
          if(friction_cone == 0){
            scivolo = 0;
            scivolo_due = 0;
            scivolo_dietro = 0;
            scivolo_dietro_due = 0;

          if(leg_up == 1)
          {
            x_wp_1 = pos_foot_RH(0) + delta_x_wp1_RH;
            y_wp_1 = pos_foot_RH(1) + delta_y_wp1_RH;
            z_wp_1 = pos_foot_RH(2) + delta_h_wp1_RH;

            Inverse_Kinematics_RH(x_wp_1,y_wp_1,z_wp_1,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RH(0) = q1_f_RH;
              q_i_RH(1) = q2_f_RH;
              q_i_RH(2) = q3_f_RH;
              contatore = 0;
              leg_up = 0;

            }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_RH;
            y_wp_2 = y_wp_1 + delta_y_wp2_RH;
            z_wp_2 = z_wp_1 + delta_h_wp2_RH;

            Inverse_Kinematics_RH(x_wp_2,y_wp_2,z_wp_2,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,0,1,0;

            }
          }
        }

        else{
          if(friction_cone == 1){

          if(leg_up == 1)
          {
            friction_cone = 1;
            x_wp_1 = pos_foot_RH(0) + delta_x_wp1_RHH;
            y_wp_1 = pos_foot_RH(1) + delta_y_wp1_RH;
            z_wp_1 = pos_foot_RH(2) + delta_h_wp1_RHH;

            Inverse_Kinematics_RH(x_wp_1,y_wp_1,z_wp_1,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RH(0) = q1_f_RH;
              q_i_RH(1) = q2_f_RH;
              q_i_RH(2) = q3_f_RH;
              contatore = 0;
              leg_up = 0;

             friction_cone = 1;

            }
          }

          else
          {
            friction_cone = 1;
            x_wp_2 = x_wp_1 + delta_x_wp2_RHH; //delta_x_wp2_RH_fd;
            y_wp_2 = y_wp_1 + delta_y_wp2_RH;
            z_wp_2 = z_wp_1 + delta_h_wp2_RHH;

            Inverse_Kinematics_RH(x_wp_2,y_wp_2,z_wp_2,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,0,1,0;
               friction_cone = 1;

            }
          }
        }

        }
        }

       else if(move_leg(2) == 1)
        {
          scivolo = 0;
          scivolo_due = 0;
          scivolo_dietro = 0;
          scivolo_dietro_due = 0;
          if(friction_cone == 0){
          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_RF(0) + delta_x_wp1_RF;
            y_wp_1 = pos_foot_RF(1) + delta_y_wp1_RF;
            z_wp_1 = pos_foot_RF(2) + delta_h_wp1_RF;

            Inverse_Kinematics_RF(x_wp_1,y_wp_1,z_wp_1,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RF(0) = q1_f_RF;
              q_i_RF(1) = q2_f_RF;
              q_i_RF(2) = q3_f_RF;
              contatore = 0;
              leg_up = 0;

          }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_RF;
            y_wp_2 = y_wp_1 + delta_y_wp2_RF;
            z_wp_2 = z_wp_1 + delta_h_wp2_RF;

            Inverse_Kinematics_RF(x_wp_2,y_wp_2,z_wp_2,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,1,0,0;
              c_switch = 0;
              base_dx = 0;
              //endd = 0;
              conta_friction = 0;

               x_time = x_wp_dx; // * cos(theta) + z_wp_dx * sin(theta); ;
              y_time = y_wp_dx;
              z_time = z_wp_dx; // * cos(theta) - x_wp_dx * sin(theta);;
              roll_time = roll_wp_dx;
              pitch_time = pitch_wp_dx;
              yaw_time = yaw_wp_dx;

              delta_x_wp1_L = delta_x_wp1_R;
              delta_x_wp2_L = delta_x_wp2_R;
            }
          }
        }
        else{
          if(friction_cone == 1){
           if(leg_up == 1)
          {
            friction_cone = 1;
            scivolo = 0;
          scivolo_due = 0;
          scivolo_dietro = 0;
          scivolo_dietro_due = 0;

            x_wp_1 = pos_foot_RF(0) + delta_x_wp1_RF;
            y_wp_1 = pos_foot_RF(1) + delta_y_wp1_RFF;
            z_wp_1 = pos_foot_RF(2) + delta_h_wp1_RFF;

            Inverse_Kinematics_RF(x_wp_1,y_wp_1,z_wp_1,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RF(0) = q1_f_RF;
              q_i_RF(1) = q2_f_RF;
              q_i_RF(2) = q3_f_RF;
              contatore = 0;
              leg_up = 0;

          }
          }

         else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_RF;
            y_wp_2 = y_wp_1 + delta_y_wp2_RFF;
            z_wp_2 = z_wp_1 + delta_h_wp2_RFF;

            Inverse_Kinematics_RF(x_wp_2,y_wp_2,z_wp_2,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,1,0,0;
              c_switch = 0;
              base_dx = 0;
              endd = 1;
              conta_friction++;
              //friction_cone = 0;

               x_time = x_wp_dx; // * cos(theta) + z_wp_dx * sin(theta); ;
              y_time = y_wp_dx;
              z_time = z_wp_dx; // * cos(theta) - x_wp_dx * sin(theta);;
              roll_time = roll_wp_dx;
              pitch_time = pitch_wp_dx;
              yaw_time = yaw_wp_dx;

              delta_x_wp1_L = delta_x_wp1_R;
              delta_x_wp2_L = delta_x_wp2_R;
            }
          }
        }

        }
      }
      }

      else if(c_switch == 0 && t > 2.1 && base_dx == 1)
      {

        move_base(x_wp_sx,y_wp_sx,z_wp_sx,roll_wp_sx,pitch_wp_sx,yaw_wp_sx,x_wp_dx,y_wp_dx,z_wp_dx,roll_wp_dx,pitch_wp_dx,yaw_wp_dx,x_time, y_time, z_time, roll_time, pitch_time, yaw_time,contatore,time_f_base);

        contatore = contatore + 0.01;

        if(contatore > time_f_base + 0.01)
        {
            contatore = 0;
            c_switch = 1;

            x_wp_sx = x_wp_dx + delta_x;

            q1_time_LF = q_i_LF(0);
            q2_time_LF = q_i_LF(1);
            q3_time_LF = q_i_LF(2);

            q1_time_LH = q_i_LH(0);
            q2_time_LH = q_i_LH(1);
            q3_time_LH = q_i_LH(2);

            q1_time_RF = q_i_RF(0);
            q2_time_RF = q_i_RF(1);
            q3_time_RF = q_i_RF(2);

            q1_time_RH = q_i_RH(0);
            q2_time_RH = q_i_RH(1);
            q3_time_RH = q_i_RH(2);

        }
      }

      else if(c_switch == 0 && t > 2.1 && base_dx == 0)
      {

        move_base(x_wp_dx,y_wp_dx,z_wp_dx,roll_wp_dx,pitch_wp_dx,yaw_wp_dx,x_wp_sx,y_wp_sx,z_wp_sx,roll_wp_sx,pitch_wp_sx,yaw_wp_sx,x_time, y_time, z_time, roll_time, pitch_time, yaw_time,contatore,time_f_base);

        contatore = contatore + 0.01;

        if(contatore > time_f_base + 0.01)
        {
            contatore = 0;
            c_switch = 1;

            x_wp_dx = x_wp_sx + delta_x;

            q1_time_LF = q_i_LF(0);
            q2_time_LF = q_i_LF(1);
            q3_time_LF = q_i_LF(2);

            q1_time_LH = q_i_LH(0);
            q2_time_LH = q_i_LH(1);
            q3_time_LH = q_i_LH(2);

            q1_time_RF = q_i_RF(0);
            q2_time_RF = q_i_RF(1);
            q3_time_RF = q_i_RF(2);

            q1_time_RH = q_i_RH(0);
            q2_time_RH = q_i_RH(1);
            q3_time_RH = q_i_RH(2);

        }
      }

      base_state.name.resize(6);
      base_state.position.resize(6);
      base_state.velocity.resize(6);
      base_state.effort.resize(6);

      joint_state.name.resize(12);
      joint_state.position.resize(12);
      joint_state.velocity.resize(12);
      joint_state.effort.resize(12);

      joint_state.name[0]="LF_HAA";
      joint_state.position[0] = q1_time_LF;
      joint_state.velocity[0] = 0.0;
      joint_state.effort[0] = 0.0;

      joint_state.name[1]="LF_HFE";
      joint_state.position[1] = q2_time_LF;
      joint_state.velocity[1] = 0.0;
      joint_state.effort[1] = 0.0;

      joint_state.name[2]="LF_KFE";
      joint_state.position[2] = q3_time_LF;
      joint_state.velocity[2] = 0.0;
      joint_state.effort[2] = 0.0;

      joint_state.name[3]="LH_HAA";
      joint_state.position[3] = q1_time_LH;
      joint_state.effort[3] = 0.0;

      joint_state.name[4]="LH_HFE";
      joint_state.position[4] = q2_time_LH;
      joint_state.velocity[4] = 0.0;
      joint_state.effort[4] = 0.0;

      joint_state.name[5]="LH_KFE";
      joint_state.position[5] = q3_time_LH;
      joint_state.velocity[5] = 0.0;
      joint_state.effort[5] = 0.0;

      joint_state.name[6]="RF_HAA";
      joint_state.position[6] = q1_time_RF;
      joint_state.velocity[6] = 0.0;
      joint_state.effort[6] = 0.0;

      joint_state.name[7]="RF_HFE";
      joint_state.position[7] = q2_time_RF;
      joint_state.velocity[7] = 0.0;
      joint_state.effort[7] = 0.0;

      joint_state.name[8]="RF_KFE";
      joint_state.position[8] = q3_time_RF;
      joint_state.effort[8] = 0.0;

      joint_state.name[9]="RH_HAA";
      joint_state.position[9] = q1_time_RH;
      joint_state.velocity[9] = 0.0;
      joint_state.effort[9] = 0.0;

      joint_state.name[10]="RH_HFE";
      joint_state.position[10] = q2_time_RH;
      joint_state.velocity[10] = 0.0;
      joint_state.effort[10] = 0.0;

      joint_state.name[11]="RH_KFE";
      joint_state.position[11] =  q3_time_RH;
      joint_state.velocity[11] = 0.0;
      joint_state.effort[11] = 0.0;

      base_state.name[0] = " x ";
      base_state.position[0] = x_time;
      base_state.velocity[0] = 0.0;
      base_state.effort[0] = 0.0;

      base_state.name[1] = " y ";
      base_state.position[1] = y_time;
      base_state.velocity[1] = 0.0;
      base_state.effort[1] = 0.0;

      base_state.name[2] = " z ";
      base_state.position[2] = z_time;
      base_state.velocity[2] = 0.0;
      base_state.effort[2] = 0.0;

      base_state.name[3] = " roll ";
      base_state.position[3] = roll_time;
      base_state.velocity[3] = 0.0;
      base_state.effort[3] = 0.0;

      base_state.name[4] = " pitch ";
      base_state.position[4] = pitch_time;
      base_state.velocity[4] = 0.0;
      base_state.effort[4] = 0.0;

      base_state.name[5] = " yaw ";
      base_state.position[5] = yaw_time;
      base_state.velocity[5] = 0.0;
      base_state.effort[5] = 0.0;

      base_pub.publish(base_state);
      joint_pub.publish(joint_state);

      c_switch_msg.data = c_switch;
      c_switch_pub.publish(c_switch_msg);

      fine_msg.data = fine;
      fine_pub.publish(fine_msg);

      end_msg.data = endd;
      end_pub.publish(end_msg);

      scivolo_msg.data = scivolo;
      scivolo_pub.publish(scivolo_msg);

       std_msgs::Float64 conta_friction_msg;
      conta_friction_msg.data = conta_friction;
      conta_friction_pub.publish(conta_friction_msg);

      std_msgs::Float64 delta_x_avanti_msg;
      delta_x_avanti_msg.data = delta_x_avanti;
      delta_x_avanti_pub.publish(delta_x_avanti_msg);

      //cout<<"contatore"<<endl<< contatore<<endl;
      cout<<"time"<<endl<< t <<endl;
      cout<<"fine"<< endl << fine <<endl;
      cout<<"endd"<<endl<<endd<<endl;
      cout<<"c_switch"<<endl<<c_switch<<endl;
      cout<<"friction_cone"<<endl<<friction_cone<<endl;
      cout<<"friction_cone_dietro"<<endl<<friction_cone_dietro<<endl;
      cout<<"friction_dietro"<<endl<<friction_dietro<<endl;
      cout<<"friction_dietro_due"<<endl<<friction_dietro_due<<endl;
      cout<<"scivolo"<<endl<<scivolo<<endl;
      cout<<"scivolo_due"<<endl<<scivolo_due<<endl;
      cout<<"scivolo_dietro"<<endl<<scivolo_dietro<<endl;
      cout<<"scivolo_dietro_due"<<endl<<scivolo_dietro_due<<endl;
      cout<<"freeze"<<endl<<freeze<<endl;

      ros::spinOnce();
      loop_rate.sleep();

    }

  return 0;
}

max_stars_repo_path: src/ufficiali veri/set_command_FB.cpp

max_issues_repo_path: src/ufficiali veri/set_command_FB.cpp

max_forks_repo_path: src/ufficiali veri/set_command_FB.cpp


text:
/* simple example for using class array<>
 *
 * (C) Copyright Nicolai M. Josuttis 2001.
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * Changelog:
 * 20 Jan 2001 - Removed boolalpha use since stock GCC doesn't support it
 *               (David Abrahams)
 */

#include <iostream>
#include <boost/array.hpp>

int main()
{
    // define special type name
    typedef boost::array<float,6> Array;

    // create and initialize an array
    Array a = { { 42 } };

    // access elements
    for (unsigned i=1; i<a.size(); ++i) {
        a[i] = a[i-1]+1;
    }

    // use some common STL container operations
    std::cout << "size:     " << a.size() << std::endl;
    std::cout << "empty:    " << (a.empty() ? "true" : "false") << std::endl;
    std::cout << "max_size: " << a.max_size() << std::endl;
    std::cout << "front:    " << a.front() << std::endl;
    std::cout << "back:     " << a.back() << std::endl;
    std::cout << "elems:    ";

    // iterate through all elements
    for (Array::const_iterator pos=a.begin(); pos<a.end(); ++pos) {
        std::cout << *pos << ' ';
    }
    std::cout << std::endl;

    // check copy constructor and assignment operator
    Array b(a);
    Array c;
    c = a;
    if (a==b && a==c) {
        std::cout << "copy construction and copy assignment are OK"
                  << std::endl;
    }
    else {
        std::cout << "copy construction and copy assignment FAILED"
                  << std::endl;
    }

    return 0;  // makes Visual-C++ compiler happy
}

ext: cpp

max_stars_repo_name: randolphwong/mcsema

max_issues_repo_name: randolphwong/mcsema

max_forks_repo_name: randolphwong/mcsema


text:
#pragma once

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <cmath>
#include <cstdint>
#include <functional>
#include <limits>
#include <iostream>
#include <stdexcept>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/openmp_helpers.hpp>

namespace common_robotics_utilities
{
/// Implementation of hierarchical clustering, in both Single-link and
/// Complete-link forms. Unlike some implementations, this does not produce the
/// dendrogram of clusters - it only returns the accumulated clusters up to the
/// provided cluster distance bound. In complete-link clustering, the maximum
/// point-to-point distance in any cluster must be <= distance bound, and
/// clustering is performed by combining two items (each could be a point or an
/// existing cluster) if the maximum distance is less than distance bound.
/// Single-link clustering is produced by combining two items (each could be a
/// point or an existing cluster) if the minimum distance is less than distance
/// bound.
/// Complete-link clustering produces dense clusters, while single-link
/// clustering produces long "thin" clusters.
namespace simple_hierarchical_clustering
{
enum class ClusterStrategy { SINGLE_LINK, COMPLETE_LINK };

/// Storage for a single "item" - either an index to a single value or an index
/// to an existing cluster.
class Item
{
private:
  int64_t index_ = -1;
  bool is_cluster_ = false;

public:
  Item() : index_(-1), is_cluster_(false) {}

  Item(const int64_t index, const bool is_cluster)
      : index_(index), is_cluster_(is_cluster)
  {
    if (index_ < 0)
    {
      throw std::invalid_argument("index < 0");
    }
  }

  int64_t Index() const { return index_; }

  bool IsValue() const { return !is_cluster_; }

  bool IsCluster() const { return is_cluster_; }

  bool IsValid() const { return (index_ >= 0); }
};

/// Storage for a pair of "items" and the distance between them
class ClosestPair
{
private:
  Item first_item_;
  Item second_item_;
  double distance_ = std::numeric_limits<double>::infinity();

public:
  ClosestPair() : distance_(std::numeric_limits<double>::infinity()) {}

  ClosestPair(const Item& first_item, const Item& second_item,
              const double distance)
      : first_item_(first_item), second_item_(second_item), distance_(distance)
  {
    if (distance < 0.0)
    {
      throw std::invalid_argument("distance < 0.0");
    }
    if (!first_item_.IsValid())
    {
      throw std::invalid_argument("first_item is not valid");
    }
    if (!second_item_.IsValid())
    {
      throw std::invalid_argument("second_item is not valid");
    }
    if (first_item_.IsValue() == second_item_.IsValue())
    {
      if (first_item_.Index() == second_item_.Index())
      {
        throw std::invalid_argument("first and second items are the same");
      }
    }
  }

  const Item& FirstItem() const { return first_item_; }

  const Item& SecondItem() const { return second_item_; }

  double Distance() const { return distance_; }

  bool IsValid() const
  {
    return first_item_.IsValid() && second_item_.IsValid();
  }
};

/// Find the closest existing clusters in @param clusters, using the pairwise
/// element-to-element distances in @param distance_matrix and the strategy
/// specified by @param strategy. @return closest pair of clusters.
/// Search is performed in parallel.
inline ClosestPair GetClosestClustersParallel(
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  std::vector<ClosestPair> per_thread_closest_clusters(
      openmp_helpers::GetNumOmpThreads(), ClosestPair());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t first_cluster_idx = 0; first_cluster_idx < clusters.size();
       first_cluster_idx++)
  {
    // Skip empty clusters
    const std::vector<int64_t>& first_cluster = clusters.at(first_cluster_idx);
    if (first_cluster.size() > 0)
    {
      // Only compare against remaining clusters
      for (size_t second_cluster_idx = first_cluster_idx + 1;
           second_cluster_idx < clusters.size();
           second_cluster_idx++)
      {
        // Skip empty clusters
        const std::vector<int64_t>& second_cluster
            = clusters.at(second_cluster_idx);
        if (second_cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster1_index : first_cluster)
          {
            for (const int64_t& cluster2_index : second_cluster)
            {
              const double distance
                  = distance_matrix(cluster1_index, cluster2_index);
              minimum_distance = std::min(minimum_distance, distance);
              maximum_distance = std::max(maximum_distance, distance);
            }
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          const int32_t thread_num = openmp_helpers::GetContextOmpThreadNum();
          const double current_closest_distance
              = per_thread_closest_clusters.at(thread_num).Distance();
          if (cluster_distance < current_closest_distance)
          {
            per_thread_closest_clusters.at(thread_num)
                = ClosestPair(Item(first_cluster_idx, true),
                              Item(second_cluster_idx, true),
                              cluster_distance);
          }
        }
      }
    }
  }
  ClosestPair closest_clusters;
  for (const ClosestPair& per_thread_closest_cluster_pair
       : per_thread_closest_clusters)
  {
    if (per_thread_closest_cluster_pair.Distance()
        < closest_clusters.Distance())
    {
      closest_clusters = per_thread_closest_cluster_pair;
    }
  }
  return closest_clusters;
}

/// Find the closest existing clusters in @param clusters, using the pairwise
/// element-to-element distances in @param distance_matrix and the strategy
/// specified by @param strategy. @return closest pair of clusters.
inline ClosestPair GetClosestClustersSerial(
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  ClosestPair closest_clusters;
  for (size_t first_cluster_idx = 0; first_cluster_idx < clusters.size();
       first_cluster_idx++)
  {
    // Skip empty clusters
    const std::vector<int64_t>& first_cluster = clusters.at(first_cluster_idx);
    if (first_cluster.size() > 0)
    {
      // Only compare against remaining clusters
      for (size_t second_cluster_idx = first_cluster_idx + 1;
           second_cluster_idx < clusters.size();
           second_cluster_idx++)
      {
        // Skip empty clusters
        const std::vector<int64_t>& second_cluster
            = clusters.at(second_cluster_idx);
        if (second_cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster1_index : first_cluster)
          {
            for (const int64_t& cluster2_index : second_cluster)
            {
              const double distance
                  = distance_matrix(cluster1_index, cluster2_index);
              minimum_distance = std::min(minimum_distance, distance);
              maximum_distance = std::max(maximum_distance, distance);
            }
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          if (cluster_distance < closest_clusters.Distance())
          {
            closest_clusters = ClosestPair(Item(first_cluster_idx, true),
                                           Item(second_cluster_idx, true),
                                           cluster_distance);
          }
        }
      }
    }
  }
  return closest_clusters;
}

/// Find the closest existing clusters in @param clusters, using the pairwise
/// element-to-element distances in @param distance_matrix and the strategy
/// specified by @param strategy. @return closest pair of clusters.
/// @param use_parallel selects if the search should be performed in parallel.
inline ClosestPair GetClosestClusters(
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy,
    const bool use_parallel)
{
  if (use_parallel)
  {
    return GetClosestClustersParallel(distance_matrix, clusters, strategy);
  }
  else
  {
    return GetClosestClustersSerial(distance_matrix, clusters, strategy);
  }
}

/// Find the closest value-{value, cluster} pair, using @param datapoint_mask to
/// ignore values that have already been clustered, @param distance_matrix to
/// provide pairwise value-to-value distances, existing clusters provided by
/// @param clusters, and strategy specified by @param strategy.
/// Search is performed in parallel. @return closest pair.
inline ClosestPair GetClosestValueToOtherParallel(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  std::vector<ClosestPair> per_thread_closest_value_other(
      openmp_helpers::GetNumOmpThreads(), ClosestPair());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t value_idx = 0; value_idx < datapoint_mask.size(); value_idx++)
  {
    // Make sure we're not already clustered
    if (datapoint_mask.at(value_idx) == 0x00)
    {
      const int32_t thread_num = openmp_helpers::GetContextOmpThreadNum();
      // Check against other values
      for (size_t other_value_idx = value_idx + 1;
           other_value_idx < datapoint_mask.size(); other_value_idx++)
      {
        // Make sure it's not already clustered
        if (datapoint_mask.at(other_value_idx) == 0x00)
        {
          const double distance = distance_matrix(value_idx, other_value_idx);
          const double current_closest_distance
              = per_thread_closest_value_other.at(thread_num).Distance();
          if (distance < current_closest_distance)
          {
            per_thread_closest_value_other.at(thread_num)
                = ClosestPair(Item(value_idx, false),
                              Item(other_value_idx, false),
                              distance);
          }
        }
      }
      // Check against clusters
      for (size_t cluster_idx = 0; cluster_idx < clusters.size(); cluster_idx++)
      {
        const std::vector<int64_t>& cluster = clusters.at(cluster_idx);
        // Skip empty clusters
        if (cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster_element_idx : cluster)
          {
            const double distance
                = distance_matrix(value_idx, cluster_element_idx);
            minimum_distance = std::min(minimum_distance, distance);
            maximum_distance = std::max(maximum_distance, distance);
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          const double current_closest_distance
              = per_thread_closest_value_other.at(thread_num).Distance();
          if (cluster_distance < current_closest_distance)
          {
            per_thread_closest_value_other.at(thread_num)
                = ClosestPair(Item(value_idx, false),
                              Item(cluster_idx, true),
                              cluster_distance);
          }
        }
      }
    }
  }
  ClosestPair closest_value_other;
  for (const ClosestPair& value_other : per_thread_closest_value_other)
  {
    if (value_other.Distance() < closest_value_other.Distance())
    {
      closest_value_other = value_other;
    }
  }
  return closest_value_other;
}

/// Find the closest value-{value, cluster} pair, using @param datapoint_mask to
/// ignore values that have already been clustered, @param distance_matrix to
/// provide pairwise value-to-value distances, existing clusters provided by
/// @param clusters, and strategy specified by @param strategy. @return closest
/// pair.
inline ClosestPair GetClosestValueToOtherSerial(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy)
{
  ClosestPair closest_value_other;
  for (size_t value_idx = 0; value_idx < datapoint_mask.size(); value_idx++)
  {
    // Make sure we're not already clustered
    if (datapoint_mask.at(value_idx) == 0x00)
    {
      // Check against other values
      for (size_t other_value_idx = value_idx + 1;
           other_value_idx < datapoint_mask.size(); other_value_idx++)
      {
        // Make sure it's not already clustered
        if (datapoint_mask.at(other_value_idx) == 0x00)
        {
          const double distance = distance_matrix(value_idx, other_value_idx);
          if (distance < closest_value_other.Distance())
          {
            closest_value_other = ClosestPair(Item(value_idx, false),
                                              Item(other_value_idx, false),
                                              distance);
          }
        }
      }
      // Check against clusters
      for (size_t cluster_idx = 0; cluster_idx < clusters.size(); cluster_idx++)
      {
        const std::vector<int64_t>& cluster = clusters.at(cluster_idx);
        // Skip empty clusters
        if (cluster.size() > 0)
        {
          // Compute cluster-cluster distance
          double minimum_distance = std::numeric_limits<double>::infinity();
          double maximum_distance = 0.0;
          for (const int64_t& cluster_element_idx : cluster)
          {
            const double distance
                = distance_matrix(value_idx, cluster_element_idx);
            minimum_distance = std::min(minimum_distance, distance);
            maximum_distance = std::max(maximum_distance, distance);
          }
          const double cluster_distance
              = (strategy == ClusterStrategy::COMPLETE_LINK) ? maximum_distance
                                                             : minimum_distance;
          if (cluster_distance < closest_value_other.Distance())
          {
            closest_value_other = ClosestPair(Item(value_idx, false),
                                              Item(cluster_idx, true),
                                              cluster_distance);
          }
        }
      }
    }
  }
  return closest_value_other;
}

/// Find the closest value-{value, cluster} pair, using @param datapoint_mask to
/// ignore values that have already been clustered, @param distance_matrix to
/// provide pairwise value-to-value distances, existing clusters provided by
/// @param clusters, and strategy specified by @param strategy.
/// @param use_parallel selects if the search should be performed in parallel.
/// @return closest pair.
inline ClosestPair GetClosestValueToOther(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy,
    const bool use_parallel)
{
  if (use_parallel)
  {
    return GetClosestValueToOtherParallel(
        datapoint_mask, distance_matrix, clusters, strategy);
  }
  else
  {
    return GetClosestValueToOtherSerial(
        datapoint_mask, distance_matrix, clusters, strategy);
  }
}

/// Find the closest {value, cluster}-{value, cluster} pair, using @param
/// datapoint_mask to ignore values that have already been clustered, @param
/// distance_matrix to provide pairwise value-to-value distances, existing
/// clusters provided by @param clusters, and strategy specified by @param
/// strategy. @param use_parallel selects if the search should be performed in
/// parallel. @return closest pair.
inline ClosestPair GetClosestPair(
    const std::vector<uint8_t>& datapoint_mask,
    const Eigen::MatrixXd& distance_matrix,
    const std::vector<std::vector<int64_t>>& clusters,
    const ClusterStrategy strategy,
    const bool use_parallel)
{
  const ClosestPair closest_value_to_other
      = GetClosestValueToOther(datapoint_mask, distance_matrix, clusters,
                               strategy, use_parallel);
  const ClosestPair closest_clusters
      = GetClosestClusters(distance_matrix, clusters, strategy, use_parallel);
  if (closest_value_to_other.IsValid() && closest_clusters.IsValid())
  {
    if (closest_value_to_other.Distance() < closest_clusters.Distance())
    {
      return closest_value_to_other;
    }
    else
    {
      return closest_clusters;
    }
  }
  else if (closest_value_to_other.IsValid())
  {
    return closest_value_to_other;
  }
  else if (closest_clusters.IsValid())
  {
    return closest_clusters;
  }
  else
  {
    return ClosestPair();
  }
}

template<typename DataType, typename Container=std::vector<DataType>>
class ClusteringResult
{
private:
  std::vector<Container> clusters_;
  double final_closest_distance_ = 0.0;

public:
  ClusteringResult() {}

  ClusteringResult(const std::vector<Container>& clusters,
                   const double final_closest_distance)
      : clusters_(clusters), final_closest_distance_(final_closest_distance) {}

  const std::vector<Container>& Clusters() const { return clusters_; }

  double FinalClosestDistance() const { return final_closest_distance_; }
};

using IndexClusteringResult = ClusteringResult<int64_t, std::vector<int64_t>>;

/// Perform hierarchical index clustering of @param data up to @param
/// max_cluster_distance, using @param distance_matrix to provide pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
inline IndexClusteringResult IndexClusterWithDistanceMatrix(
    const Eigen::MatrixXd& distance_matrix, const double max_cluster_distance,
    const ClusterStrategy strategy, const bool use_parallel = false)
{
  if (distance_matrix.rows() != distance_matrix.cols())
  {
    throw std::invalid_argument("distance_matrix is not square");
  }
  else if (distance_matrix.rows() == 0)
  {
    throw std::invalid_argument("distance_matrix is empty");
  }
  std::vector<uint8_t> datapoint_mask(distance_matrix.rows(), 0u);
  std::vector<std::vector<int64_t>> cluster_indices;
  double closest_distance = 0.0;
  bool complete = false;
  while (!complete)
  {
    // Get closest pair of items (an element can be a cluster or single value!)
    const ClosestPair closest_element_pair
        = GetClosestPair(datapoint_mask, distance_matrix, cluster_indices,
                         strategy, use_parallel);
    if (closest_element_pair.IsValid()
        && closest_element_pair.Distance() <= max_cluster_distance)
    {
      closest_distance = closest_element_pair.Distance();
      const auto& first_item = closest_element_pair.FirstItem();
      const auto& second_item = closest_element_pair.SecondItem();
      // If both elements are values, create a new cluster
      if (first_item.IsValue() && second_item.IsValue())
      {
        // Add a cluster
        cluster_indices.push_back(std::vector<int64_t>{first_item.Index(),
                                                       second_item.Index()});
        // Mask out the indices (this way we know they are already clustered)
        datapoint_mask.at(static_cast<size_t>(first_item.Index())) = 1u;
        datapoint_mask.at(static_cast<size_t>(second_item.Index())) = 1u;
      }
      // If both elements are clusters, merge the clusters
      else if (first_item.IsCluster() && second_item.IsCluster())
      {
        // Merge the second cluster into the first
        std::vector<int64_t>& first_cluster
            = cluster_indices.at(static_cast<size_t>(first_item.Index()));
        std::vector<int64_t>& second_cluster
            = cluster_indices.at(static_cast<size_t>(second_item.Index()));
        first_cluster.insert(
            first_cluster.end(), second_cluster.begin(), second_cluster.end());
        // Empty the second cluster
        // (we don't remove, because this would trigger a move and reallocation)
        second_cluster.clear();
      }
      // If one of the elements is a cluster and the other is a point, add the
      // point to the existing cluster
      else
      {
        const bool first_is_cluster = first_item.IsCluster();
        const int64_t cluster_index
            = (first_is_cluster) ? first_item.Index() : second_item.Index();
        const int64_t value_index
            = (first_is_cluster) ? second_item.Index() : first_item.Index();
        // Add the element to the cluster
        std::vector<int64_t>& cluster
            = cluster_indices.at(static_cast<size_t>(cluster_index));
        cluster.push_back(value_index);
        // Mask out the index (this way we know it is are already clustered)
        datapoint_mask.at(static_cast<size_t>(value_index)) = 1u;
      }
    }
    else
    {
      complete = true;
    }
  }
  // Get rid of empty clusters left over from cluster-cluster merges
  std::vector<std::vector<int64_t>> index_clusters;
  index_clusters.reserve(cluster_indices.size());
  for (const auto& index_cluster : cluster_indices)
  {
    if (index_cluster.size() > 0)
    {
      index_clusters.push_back(index_cluster);
    }
  }
  // Add any points that we haven't clustered into their own clusters
  for (size_t idx = 0; idx < datapoint_mask.size(); idx++)
  {
    // If an element hasn't been clustered at all yet, make a new cluster
    if (datapoint_mask.at(idx) == 0)
    {
      index_clusters.push_back(
          std::vector<int64_t>(1, static_cast<int64_t>(idx)));
    }
  }
  index_clusters.shrink_to_fit();
  return IndexClusteringResult(index_clusters, closest_distance);
}

template<typename DataType, typename Container=std::vector<DataType>>
inline ClusteringResult<DataType, Container>
MakeElementClusteringFromIndexClustering(
    const Container& data,
    const IndexClusteringResult& index_clustering)
{
  std::vector<Container> clusters(index_clustering.Clusters().size());
  for (size_t idx = 0; idx < clusters.size(); idx++)
  {
    const std::vector<int64_t>& current_index_cluster
        = index_clustering.Clusters().at(idx);
    Container& current_cluster = clusters.at(idx);
    // Use reserve + shrink_to_fit for cases where DataType is not
    // default-constructible.
    current_cluster.reserve(current_index_cluster.size());
    for (const int64_t data_index : current_index_cluster)
    {
      current_cluster.push_back(data.at(static_cast<size_t>(data_index)));
    }
    current_cluster.shrink_to_fit();
  }
  return ClusteringResult<DataType, Container>(
      clusters, index_clustering.FinalClosestDistance());
}

/// Perform hierarchical clustering of @param data up to @param
/// max_cluster_distance, using @param distance_matrix to provide pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
template<typename DataType, typename Container=std::vector<DataType>>
inline ClusteringResult<DataType, Container> ClusterWithDistanceMatrix(
    const Container& data, const Eigen::MatrixXd& distance_matrix,
    const double max_cluster_distance, const ClusterStrategy strategy,
    const bool use_parallel = false)
{
  // Safety check the input
  if (data.empty())
  {
    throw std::invalid_argument("data is empty");
  }
  if (static_cast<size_t>(distance_matrix.rows()) != data.size()
      || static_cast<size_t>(distance_matrix.cols()) != data.size())
  {
    throw std::invalid_argument("distance_matrix is the wrong size");
  }
  // Perform index clustering
  const auto index_clustering = IndexClusterWithDistanceMatrix(
      distance_matrix, max_cluster_distance, strategy, use_parallel);
  // Extract the actual cluster data from index clusters
  return MakeElementClusteringFromIndexClustering<DataType, Container>(
      data, index_clustering);
}

/// Perform hierarchical index clustering of @param data up to @param
/// max_cluster_distance, using @param distance_fn to compute pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
template<typename DataType, typename Container=std::vector<DataType>>
inline IndexClusteringResult IndexCluster(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn,
    const double max_cluster_distance, const ClusterStrategy strategy,
    const bool use_parallel = false)
{
  const Eigen::MatrixXd distance_matrix
      = math::BuildPairwiseDistanceMatrix<DataType, Container>(
          data, distance_fn, use_parallel);
  return IndexClusterWithDistanceMatrix(
      distance_matrix, max_cluster_distance, strategy, use_parallel);
}

/// Perform hierarchical clustering of @param data up to @param
/// max_cluster_distance, using @param distance_fn to compute pairwise
/// value-to-value distance for values in @param data. Strategy to use is
/// specified by @param strategy, and @param use_parallel selects if parallel
/// search should be used internally.
template<typename DataType, typename Container=std::vector<DataType>>
inline ClusteringResult<DataType, Container> Cluster(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn,
    const double max_cluster_distance, const ClusterStrategy strategy,
    const bool use_parallel = false)
{
  const Eigen::MatrixXd distance_matrix
      = math::BuildPairwiseDistanceMatrix<DataType, Container>(
          data, distance_fn, use_parallel);
  return ClusterWithDistanceMatrix<DataType, Container>(
      data, distance_matrix, max_cluster_distance, strategy, use_parallel);
}
}  // namespace simple_hierarchical_clustering
}  // namespace common_robotics_utilities

ext: hpp


text:
// Boost.Range library
//
//  Copyright Neil Groves 2014. Use, modification and
//  distribution is subject to the Boost Software License, Version
//  1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/range/adaptor/type_erased.hpp>
#include "type_erased_test.hpp"

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <vector>

namespace boost_range_adaptor_type_erased_test
{
    namespace
    {

void template_parameter_conversion()
{
    typedef boost::any_range<
                int
              , boost::random_access_traversal_tag
              , int&
              , std::ptrdiff_t
    > source_range_type;

    typedef boost::any_range<
                int
              , boost::single_pass_traversal_tag
              , const int&
              , std::ptrdiff_t
    > target_range_type;

    source_range_type source;

    // Converting via construction
    target_range_type t1(source);

    // Converting via assignment
    target_range_type t2;
    t2 = source;

    // Converting via construction to a type with a reference type
    // that is a value
    typedef boost::any_range<
                int
              , boost::single_pass_traversal_tag
              , int
              , std::ptrdiff_t
            > target_range2_type;

    target_range2_type t3(source);
    target_range2_type t4;
    t4 = source;
}

    } // anonymous namespace
} // namespace boost_range_adaptor_type_erased_test

boost::unit_test::test_suite*
init_unit_test_suite(int argc, char* argv[])
{
    boost::unit_test::test_suite* test =
        BOOST_TEST_SUITE("RangeTestSuite.adaptor.type_erased_tparam_conv");

    test->add(BOOST_TEST_CASE(
        &boost_range_adaptor_type_erased_test::template_parameter_conversion));

    return test;
}