text:
// Copyright 2012 Cloudera Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "util/cgroups-mgr.h"

#include <fstream>
#include <sstream>
#include <boost/filesystem.hpp>
#include "util/debug-util.h"
#include <gutil/strings/substitute.h>

using namespace impala;
using namespace std;
using namespace boost::filesystem;
using namespace boost;
using namespace strings;

namespace impala {

// Suffix appended to Yarn resource ids to form an Impala-internal cgroups.
const std::string IMPALA_CGROUP_SUFFIX = "_impala";

// Yarn's default multiplier for translating virtual CPU cores into cgroup CPU shares.
// See Yarn's CgroupsLCEResourcesHandler.java for more details.
const int32_t CPU_DEFAULT_WEIGHT = 1024;

CgroupsMgr::CgroupsMgr(Metrics* metrics) {
  active_cgroups_metric_ =
      metrics->CreateAndRegisterPrimitiveMetric<int64_t>(
          "cgroups-mgr.active-cgroups", 0);
}

Status CgroupsMgr::Init(const string& cgroups_hierarchy_path,
      const string& staging_cgroup) {
  cgroups_hierarchy_path_ = cgroups_hierarchy_path;
  staging_cgroup_ = staging_cgroup;
  // Set up the staging cgroup for Impala to retire execution threads into.
  RETURN_IF_ERROR(CreateCgroup(staging_cgroup, true));
  return Status::OK;
}

string CgroupsMgr::UniqueIdToCgroup(const string& unique_id) const {
  if (unique_id.empty()) return "";
  return unique_id + IMPALA_CGROUP_SUFFIX;
}

int32_t CgroupsMgr::VirtualCoresToCpuShares(int16_t v_cpu_cores) {
  if (v_cpu_cores <= 0) return -1;
  return CPU_DEFAULT_WEIGHT * v_cpu_cores;
}

Status CgroupsMgr::CreateCgroup(const string& cgroup, bool if_not_exists) const {
  const string& cgroup_path = Substitute("$0/$1", cgroups_hierarchy_path_, cgroup);
  try {
    // Returns false if the dir already exists, otherwise throws an exception.
    if (!create_directory(cgroup_path) && !if_not_exists) {
      stringstream err_msg;
      err_msg << "Failed to create CGroup at path " << cgroup_path
              << ". Path already exists.";
      return Status(err_msg.str());
    }
    LOG(INFO) << "Created CGroup " << cgroup_path;
  } catch (std::exception& e) {
    stringstream err_msg;
    err_msg << "Failed to create CGroup at path " << cgroup_path << ". " << e.what();
    return Status(err_msg.str());
  }
  return Status::OK;
}

Status CgroupsMgr::DropCgroup(const string& cgroup, bool if_exists) const {
  const string& cgroup_path = Substitute("$0/$1", cgroups_hierarchy_path_, cgroup);
  LOG(INFO) << "Dropping CGroup " << cgroups_hierarchy_path_ << " " << cgroup;
  try {
    if(!remove(cgroup_path) && !if_exists) {
      stringstream err_msg;
      err_msg << "Failed to create CGroup at path " << cgroup_path
              << ". Path does not exist.";
      return Status(err_msg.str());
    }
  } catch (std::exception& e) {
    stringstream err_msg;
    err_msg << "Failed to drop CGroup at path " << cgroup_path << ". " << e.what();
    return Status(err_msg.str());
  }
  return Status::OK;
}

Status CgroupsMgr::SetCpuShares(const string& cgroup, int32_t num_shares) {
  string cgroup_path;
  string tasks_path;
  RETURN_IF_ERROR(GetCgroupPaths(cgroup, &cgroup_path, &tasks_path));

  const string& cpu_shares_path = Substitute("$0/$1", cgroup_path, "cpu.shares");
  ofstream cpu_shares(tasks_path.c_str(), ios::out | ios::trunc);
  if (!cpu_shares.is_open()) {
    stringstream err_msg;
    err_msg << "CGroup CPU shares file: " << cpu_shares_path
            << " is not writable by Impala";
    return Status(err_msg.str());
  }

  LOG(INFO) << "Setting CPU shares of CGroup " << cgroup_path << " to " << num_shares;
  cpu_shares << num_shares << endl;
  return Status::OK;
}

Status CgroupsMgr::GetCgroupPaths(const std::string& cgroup,
    std::string* cgroup_path, std::string* tasks_path) const {
  stringstream cgroup_path_ss;
  cgroup_path_ss << cgroups_hierarchy_path_ << "/" << cgroup;
  *cgroup_path = cgroup_path_ss.str();
  if (!exists(*cgroup_path)) {
    stringstream err_msg;
    err_msg << "CGroup " << *cgroup_path << " does not exist";
    return Status(err_msg.str());
  }

  stringstream tasks_path_ss;
  tasks_path_ss << *cgroup_path << "/tasks";
  *tasks_path = tasks_path_ss.str();
  if (!exists(*tasks_path)) {
    stringstream err_msg;
    err_msg << "CGroup " << *cgroup_path << " does not have a /tasks file";
    return Status(err_msg.str());
  }
  return Status::OK;
}

Status CgroupsMgr::AssignThreadToCgroup(const Thread& thread,
    const string& cgroup) const {
  string cgroup_path;
  string tasks_path;
  RETURN_IF_ERROR(GetCgroupPaths(cgroup, &cgroup_path, &tasks_path));

  ofstream tasks(tasks_path.c_str(), ios::out | ios::app);
  if (!tasks.is_open()) {
    stringstream err_msg;
    err_msg << "CGroup tasks file: " << tasks_path << " is not writable by Impala";
    return Status(err_msg.str());
  }
  tasks << thread.tid() << endl;

  VLOG_ROW << "Thread " << thread.tid() << " moved to CGroup " << cgroup_path;
  tasks.close();
  return Status::OK;
}

Status CgroupsMgr::RelocateThreads(const string& src_cgroup,
    const string& dst_cgroup) const {
  string src_cgroup_path;
  string src_tasks_path;
  RETURN_IF_ERROR(GetCgroupPaths(src_cgroup, &src_cgroup_path, &src_tasks_path));

  string dst_cgroup_path;
  string dst_tasks_path;
  RETURN_IF_ERROR(GetCgroupPaths(dst_cgroup, &dst_cgroup_path, &dst_tasks_path));

  ifstream src_tasks(src_tasks_path.c_str());
  if (!src_tasks) {
    stringstream err_msg;
    err_msg << "Failed to open source CGroup tasks file at: " << src_tasks_path;
    return Status(err_msg.str());
  }

  ofstream dst_tasks(dst_tasks_path.c_str(), ios::out | ios::app);
  if (!dst_tasks) {
    stringstream err_msg;
    err_msg << "Failed to open destination CGroup tasks file at: " << dst_tasks_path;
    return Status(err_msg.str());
  }

  int32_t tid;
  while (src_tasks >> tid) {
    dst_tasks << tid << endl;
    // Attempting to write a non-existent tid/pid will result in an error,
    // so clear the error flags after every append.
    dst_tasks.clear();
    VLOG_ROW << "Relocating thread id " << tid << " from " << src_tasks_path
             << " to " << dst_tasks_path;
  }

  return Status::OK;
}

Status CgroupsMgr::RegisterFragment(const TUniqueId& fragment_instance_id,
    const string& cgroup, bool* is_first) {
  if (cgroup.empty() || cgroups_hierarchy_path_.empty()) return Status::OK;

  LOG(INFO) << "Registering fragment " << PrintId(fragment_instance_id)
            << " with CGroup " << cgroups_hierarchy_path_ << "/" << cgroup;
  lock_guard<mutex> l(active_cgroups_lock_);
  if (++active_cgroups_[cgroup] == 1) {
    *is_first = true;
    RETURN_IF_ERROR(CreateCgroup(cgroup, false));
    active_cgroups_metric_->Increment(1);
  } else {
    *is_first = false;
  }
  return Status::OK;
}

Status CgroupsMgr::UnregisterFragment(const TUniqueId& fragment_instance_id,
    const string& cgroup) {
  if (cgroup.empty() || cgroups_hierarchy_path_.empty()) return Status::OK;

  LOG(INFO) << "Unregistering fragment " << PrintId(fragment_instance_id)
            << " from CGroup " << cgroups_hierarchy_path_ << "/" << cgroup;
  lock_guard<mutex> l(active_cgroups_lock_);
  unordered_map<string, int32_t>::iterator entry = active_cgroups_.find(cgroup);
  DCHECK(entry != active_cgroups_.end());

  int32_t* ref_count = &entry->second;
  --(*ref_count);
  if (*ref_count == 0) {
    RETURN_IF_ERROR(RelocateThreads(cgroup, staging_cgroup_));
    RETURN_IF_ERROR(DropCgroup(cgroup, false));
    active_cgroups_metric_->Increment(-1);
    active_cgroups_.erase(entry);
  }
  return Status::OK;
}

}

ext: cc

max_stars_repo_name: AtScaleInc/Impala

max_issues_repo_name: AtScaleInc/Impala

max_forks_repo_name: AtScaleInc/Impala


text:
#pragma once

#include "Hook.hpp"

#include <boost\filesystem\path.hpp>

#define EXPORT extern "C"
#define VTABLE(object) (*reinterpret_cast<ReShade::Hook::Function **>(object))

namespace ReShade
{
	enum class HookType
	{
		Export,
		FunctionHook,
		VTableHook
	};

	// -----------------------------------------------------------------------------------------------------

	namespace Hooks
	{
		void Register(const boost::filesystem::path &module);
		bool Register(Hook::Function target, const Hook::Function replacement);
		bool Register(Hook::Function vtable[], unsigned int offset, const Hook::Function replacement);
		void Uninstall();

		Hook Find(const Hook::Function replacement);
		template <typename F>
		inline Hook Find(const F replacement)
		{
			return Find(reinterpret_cast<const Hook::Function>(replacement));
		}
		Hook::Function Call(const Hook::Function replacement);
		template <typename F>
		inline F Call(const F replacement)
		{
			return reinterpret_cast<F>(Call(reinterpret_cast<const Hook::Function>(replacement)));
		}
	}
}

ext: hpp

max_stars_repo_name: acourreges/reshade

max_issues_repo_name: acourreges/reshade

max_forks_repo_name: acourreges/reshade


text:
#include "DisconnectCommandHandler.h"

#include <boost/algorithm/string.hpp>

#include "jsonParserFwd.h"

using namespace rapidjson;

DisconnectCommandHandler::DisconnectCommandHandler(
		core::ClientDataPtr clientData,
		core::IDisplayerPtr displayer,
		core::IQueueHandlerPtr queueHandler,
		IRequestResponseManagerPtr responseManager,
		IMessageListenerPtr messageListener,
		MessageListenerControllerPtr listenerController)
	:_clientData(clientData),
	 _displayer(displayer),
	 _queueHandler(queueHandler),
	 _responseManager(responseManager),
	 _messageListener(messageListener),
	 _listenerController(listenerController)
{

}

DisconnectCommandHandler::~DisconnectCommandHandler()
{

}

void DisconnectCommandHandler::handle(std::string input)
{
	boost::trim(input);

	if(!input.empty())
	{
		_displayer->displayLine("Invalid count of parameters, see #help");
		return;
	}

	if (_clientData->Id > 0)
	{
		_responseManager->registerResponseType(_clientData->RequestId ,getResponseCommand());

		std::string request = "quit_server";

		rapidjson::Document responseDoc;
		responseDoc.SetObject();
		responseDoc.AddMember("user_id", Value().SetInt(_clientData->Id), responseDoc.GetAllocator());
		responseDoc.AddMember("request", Value().SetString(request.c_str() , request.length()), responseDoc.GetAllocator());
		responseDoc.AddMember("request_id", Value().SetInt(_clientData->RequestId++), responseDoc.GetAllocator());

		rapidjson::StringBuffer buffer;
		rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
		responseDoc.Accept(writer);

		_queueHandler->send(1, std::string(buffer.GetString()));
	}
	else
	{
		_displayer->displayLine("You are not connected");
	}
}

void DisconnectCommandHandler::handleResponse(rapidjson::Document& response)
{
	_listenerController->disableReading();
	//_queueHandler->send(_clientData->Id, ""); autopoke
	_clientData->Id = -1;
	_queueHandler->setQueueProvider(core::IQueueIdProviderPtr(nullptr));
	_displayer->displayLine("You are successfully disconnected");

}

std::string DisconnectCommandHandler::getCommand()
{
	return "#disconnect";
}

std::string DisconnectCommandHandler::getResponseCommand()
{
	return "#Disconnect_response";
}

std::string DisconnectCommandHandler::getInfo()
{
	return "#disconnect [disconnects from actually connected server]";
}

ext: cpp


text:
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <fstream>
#include <iostream>
#include <linux/input-event-codes.h>

#include "ros/package.h"
#include "math.h"
#include "myGlobalData.h"
#include "myFunction.h"
#include "origarm_ros/Sensor.h"
#include "origarm_ros/States.h"
#include "origarm_ros/keynumber.h"
#include "origarm_ros/Command_ABL.h"
#include "origarm_ros/Command_Pre_Open.h"
#include "origarm_ros/transdiffConfigSeg.h"
#include "origarm_ros/transdiffConfig.h"
#include "origarm_ros/transdiffDataSeg.h"
#include "origarm_ros/transFFCompensateDataSeg.h"
#include "origarm_ros/transFFCompensateData.h"
#include "origarm_ros/PIDdata.h"

#include "ros/ros.h"
#include <ros/time.h>
#include <time.h>
#include "assert.h"
#include "myPID.h"

#define DEBUG_DISPLAY 0
#define DUMMY_QUAT_USED 0

using namespace Eigen;
using Eigen::Matrix3f;
using Eigen::Matrix4f;
using Eigen::MatrixXf;
using Eigen::Quaternionf;
using Eigen::Vector3f;
using Eigen::VectorXf;

using namespace std;
static float rad2deg = 180.0f / M_PI;

static float alpha[SEGNUM];
static float beta[SEGNUM];
static float lengthAbsM[SEGNUM];

static float alphaDeg[SEGNUM];
static float betaDeg[SEGNUM];
static float lengthAbsMM[SEGNUM];

// local definition of pressure, distance, quaternion
static int16_t pressureActuator[SEGNUM][ACTNUM];
static int16_t lengthActuator[SEGNUM][ACTNUM];

static Quaternionf QuattInIMU[SEGNUM][ACTNUM];
static Quaternionf Quat0InIMU[SEGNUM][ACTNUM];

static Matrix3f Rtrans[SEGNUM][ACTNUM];
static Matrix3f R0InIMU[SEGNUM][ACTNUM], RtInIMU[SEGNUM][ACTNUM];
static Matrix3f R0InArm[SEGNUM][ACTNUM], RtInArm[SEGNUM][ACTNUM];
static Matrix3f dRInArm[SEGNUM][ACTNUM];
static Matrix3f ddRInArm[9][3][SEGNUM];

static float alphaCandidates[9][3][SEGNUM];
static float betaCandidates[9][3][SEGNUM];
static float alphaFromActuators[SEGNUM];
static float betaFromActuators[SEGNUM];

LowPassFilter ablStateFilter[SEGNUM][3];
LowPassFilter ablPassiveFilter[SEGNUM][3];

static Quaternionf quaternionPlates[SEGMENTNUM];
static Matrix3f R0PlateInArm[SEGNUM], RtPlateInArm[SEGNUM], dRPlateInArm[SEGNUM];
static float alphaFromPlates[SEGNUM];
static float betaFromPlates[SEGNUM];

/*Flag to control how the alpha is calculated*/
static const int calculate_alpha_using_pre_cur_flag = 1;
static const int calculate_alpha_using_cur_cur_flag = 0;
static const int calculate_alpha_using_cur_nex_flag = 1;

/*Path to save the IMU data at zero position*/

static string quat0DefaultFileName = "default_data_imu0.txt";
static string quattDummyFileName = "imu_move_segment0.txt";
static string quatSaveFileName = "data_imu0.txt";
static string quatReadFileName = "data_imu0.txt";
static string IMUDataPath = "";
static void InitFrames();

// abl from passive sensing
static float alpha_passive[SEGNUM];
static float beta_passive[SEGNUM];
static float length_passive[SEGNUM];
static float lengthfrompressure[SEGNUM][ACTNUM];
// static float initTransLength[SEGNUM][ACTNUM];   //directly from sensor, relative mm
// static float initTransPressure[SEGNUM][ACTNUM]; //directly from sensor, gauge HPa
static float initTransPressureComp[SEGNUM][ACTNUM];

float measureDeadzoneAlpha = 0.035; // when alphar < measureDeadzoneAlpha, betar = betaD

// pressure compensation for passive sensing actuators

static ros::Time initTransTime0[SEGNUM];
static ros::Duration initTransDuration[SEGNUM];
static float leakageRate[SEGNUM][ACTNUM] = {
    {1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 1},
    {0.0021, 1, 0.1, 1, 0.1, 1}, //interested segment
    {1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 1}};

origarm_ros::transdiffConfig transdiffConfigs;

/*Indication of whether the IMU is working well*/
static int goodIMU[SEGNUM][ACTNUM] = {
    {0, 1, 0, 0, 1, 1},
    {0, 0, 1, 0, 1, 0},
    {1, 1, 1, 0, 1, 1},
    {1, 0, 0, 1, 0, 0},
    {1, 1, 0, 1, 0, 1},
    {0, 1, 0, 0, 1, 0}};

float pressureDFeed[SEGNUM][ACTNUM];
float pressureDBack[SEGNUM][ACTNUM];

static void InitFrames();

std::string getTimeString()
{
    time_t rawtime;
    struct tm *timeinfo;
    char buffer[100];

    time(&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer, 100, "%G_%h_%d_%H_%M_%S", timeinfo);
    std::string ret = buffer;
    return ret;
}

float getPastTime(ros::Time starttime)
{
    ros::Time curtime = ros::Time::now();
    ros::Duration pasttime = curtime - starttime;
    float pastSec = pasttime.toSec();
    return pastSec;
}

/**
 * @brief Save IMU data to file
 *
 * @param filePath The path where the data is to be stored
 */
static void saveQuatToFile(Quaternionf (&qua)[SEGNUM][ACTNUM], string filePath)
{
    ofstream data;
    data.open(filePath, ios::trunc); // ios::app
    // write imu data into yaml file/imu_data.txt
    cout << "Saving current IMU data to" + filePath << endl;
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            data << qua[i][j].w() << " " << qua[i][j].x() << " " << qua[i][j].y()
                 << " " << qua[i][j].z() << endl;
            cout << qua[i][j].w() << " " << qua[i][j].x() << " " << qua[i][j].y()
                 << " " << qua[i][j].z() << endl;
        }
    }
    data.close();
    cout << "IMU data Saved" << endl;
}

/**
 * @brief read IMU data from file
 *
 * @param filePath The filepath where the original data is stored
 */
static void readQuatFromFile(string filePath, Quaternionf (&qua)[SEGNUM][ACTNUM])
{
    ifstream inFile;
    inFile.open(filePath, ios::in);
    if (inFile.fail())
    {
        inFile.close();
        cout << "unable to open the IMU file in " << filePath;
        filePath = IMUDataPath + quat0DefaultFileName;
        cout << ". The IMU default values will be used in " << filePath << endl;
        inFile.open(filePath, ios::in);
    }

    cout << "Reading IMU data from" + filePath << endl;
    if (!inFile.eof())
    {
        for (int p = 0; p < SEGNUM; p++)
        {
            for (int q = 0; q < ACTNUM; q++)
            {
                inFile >> qua[p][q].w() >> qua[p][q].x() >> qua[p][q].y() >> qua[p][q].z();
                cout << qua[p][q].w() << " " << qua[p][q].x() << " " << qua[p][q].y()
                     << " " << qua[p][q].z() << endl;
            }
        }
    }
    cout << "IMU data Read completed" << endl;
    inFile.close();
}

/*set DEBUG_DISPLAY=1 to print all the matrices*/
static void displayAllMatrix()
{

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "Rtrans Seg " << i << " Act " << j << endl;
            dispMatrix(Rtrans[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {

            cout << "R0InIMU Seg " << i << " Act " << j << endl;
            cout << Quat0InIMU[i][j].w() << " " << Quat0InIMU[i][j].x() << " " << Quat0InIMU[i][j].y() << " " << Quat0InIMU[i][j].z() << endl;
            dispMatrix(R0InIMU[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "RtInIMU Seg " << i << " Act " << j << endl;
            dispMatrix(RtInIMU[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "R0InArm Seg " << i << " Act " << j << endl;
            dispMatrix(R0InArm[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "RtInArm Seg " << i << " Act " << j << endl;
            dispMatrix(RtInArm[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "dRInArm Seg " << i << " Act " << j << endl;
            dispMatrix(dRInArm[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        cout << "alphaFromActuator Seg " << i << endl;
        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 9; k++)
            {

                cout << alphaCandidates[k][j][i] << " ";
            }
            cout << endl;
        }
    }
}

void initFilters()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        ablStateFilter[i][0].setCoeff(0.7);
        ablStateFilter[i][1].setCoeff(0.7);
        ablStateFilter[i][2].setCoeff(0.7);
        ablStateFilter[i][0].setValue(0);
        ablStateFilter[i][1].setValue(0);
        ablStateFilter[i][2].setValue(g_length0);

        ablPassiveFilter[i][0].setCoeff(0.7);
        ablPassiveFilter[i][1].setCoeff(0.7);
        ablPassiveFilter[i][2].setCoeff(0.7);
        ablPassiveFilter[i][0].setValue(0);
        ablPassiveFilter[i][1].setValue(0);
        ablPassiveFilter[i][2].setValue(g_length0);
    }
}

class State_Estimator
{
public:
    State_Estimator()
    {
        sub_ = n_.subscribe("Sensor", 300, &State_Estimator::sensorCallback, this);
        key_sub_ = n_.subscribe("key_number", 1, &State_Estimator::keyCallback, this);
        trans_sub_ = n_.subscribe("transdiffConfigSeg", 1, &State_Estimator::transdiffConfigSegCallback, this);
        cmd_sub_ = n_.subscribe("Cmd_ABL_joy", 1, &State_Estimator::ablCmdCallback, this);
        pcmd_sub_ = n_.subscribe("Command_Pre_Open", 1, &State_Estimator::pCmdCallback, this);
        pfeedforward_sub_ = n_.subscribe("Command_Pre_feedforward", 1, &State_Estimator::pfeedforwardCallback, this);
        pfeedback_sub_ = n_.subscribe("Command_Pre_feedback", 1, &State_Estimator::pfeedbackCallback, this);
        transffcompcmd_sub = n_.subscribe("transFFCompData", 1, &State_Estimator::transFFCallback, this);
        piddata_sub_ = n_.subscribe("PID_data", 1, &State_Estimator::PIDdataCallback, this);

        pub_ = n_.advertise<origarm_ros::States>("States", 300);
        pub2_ = n_.advertise<origarm_ros::transdiffConfig>("transdiffConfigurations", 100);
        pub3_ = n_.advertise<origarm_ros::transdiffDataSeg>("transdiffSegmentData", 300);
    }

    void keyCallback(const origarm_ros::keynumber &key)
    {
        if (key.keycodePressed == KEY_C) // 'C' pressed
        {
            printf(" KEY_C pressed!\r\n");
            string path = IMUDataPath + quatSaveFileName;
            saveQuatToFile(QuattInIMU, path);
            InitFrames();
        }
        else if (key.keycodePressed == KEY_R) // 'r' pressed
        {
            printf(" KEY_R pressed! Saving IMU data...\r\n");
            std::string path = IMUDataPath + "Rec_IMU_" + getTimeString() + ".txt";
            saveQuatToFile(QuattInIMU, path);
        }
    }
    void pCmdCallback(const origarm_ros::Command_Pre_Open &msg)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                Cmd_pressure.segment[i].command[j].pressure = msg.segment[i].command[j].pressure;
                Cmd_pressure.segment[i].command[j].valve = msg.segment[i].command[j].valve;
            }
        }
    }
    void ablCmdCallback(const origarm_ros::Command_ABL &ablcmd)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            ablCommand.segment[i].A = ablcmd.segment[i].A;
            ablCommand.segment[i].B = ablcmd.segment[i].B;
            ablCommand.segment[i].L = ablcmd.segment[i].L;
        }
    }
    void sensorCallback(const origarm_ros::Sensor &Sensor_)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                pressureActuator[i][j] = Sensor_.sensor_segment[i].sensor_actuator[j].pressure;
                lengthActuator[i][j] = Sensor_.sensor_segment[i].sensor_actuator[j].distance;
                QuattInIMU[i][j].w() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.w; // double check
                QuattInIMU[i][j].x() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.x;
                QuattInIMU[i][j].y() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.y;
                QuattInIMU[i][j].z() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.z;
                RtInIMU[i][j] = QuattInIMU[i][j];
            }
        }
    }

    void transdiffConfigSegCallback(const origarm_ros::transdiffConfigSeg &msg)
    {
        int segn = msg.transSegment;
        int ret = 0;
        int dataGood = 1;
        int existi[ACTNUM] = {0, 0, 0, 0, 0, 0};
        int existSum = 0;

        //confirm correct and consistent data
        if (!((msg.transSegment >= 0) && (msg.transSegment < SEGNUM) && (msg.transMode >= 0) && (msg.transMode <= 2) && (msg.transActuatorNum >= 0) && (msg.transPassiveNum >= 0) && (msg.transActuatorNum + msg.transPassiveNum <= 6)))
            dataGood = 0;

        if (msg.transMode == 0 && msg.transActuatorNum != ACTNUM)
        {
            dataGood = 0;
        }
        if (msg.transMode == 2 && msg.transActuatorNum == ACTNUM)
        {
            dataGood = 0;
        }
        for (int i = 0; i < ACTNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                if (i == msg.transActuatorIndex[j])
                {
                    existi[i] = 1;
                }
            }
        }
        // printf("existi= %d %d %d %d %d %d\n", existi[0], existi[1], existi[2], existi[3], existi[4], existi[5]);
        for (int i = 0; i < ACTNUM; i++)
        {
            existSum += existi[i];
        }
        if (existSum != 6)
        {
            dataGood = 0;
        }
        if (dataGood)
        {
            transdiffConfigs.transdiff_Configuration[segn].transMode = msg.transMode;
            transdiffConfigs.transdiff_Configuration[segn].transSegment = msg.transSegment;
            transdiffConfigs.transdiff_Configuration[segn].transActuatorNum = msg.transActuatorNum;
            transdiffConfigs.transdiff_Configuration[segn].transPassiveNum = msg.transPassiveNum;
            for (int j = 0; j < ACTNUM; j++)
            {
                transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[j] = msg.transActuatorIndex[j];
                if (msg.transMode == 2)
                {
                    transdiffConfigs.transdiff_Configuration[segn].transInitLength[j] = lengthActuator[segn][j];
                    transdiffConfigs.transdiff_Configuration[segn].transInitPressure[j] = pressureActuator[segn][j];
                    initTransTime0[segn] = ros::Time::now(); // for pressure compensation
                }
            }
            pub2_.publish(transdiffConfigs);
            ROS_INFO("Good Transdifferentiation configuration Command");
        }
        else
        {
            ROS_INFO("Incorrect data of Transdifferentiation configuration Command");
        }
    }

    void pfeedforwardCallback(const origarm_ros::Command_Pre_Open &msg)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                pressureDFeed[i][j] = msg.segment[i].command[j].pressure;
            }
        }
    }

    void pfeedbackCallback(const origarm_ros::Command_Pre_Open &msg)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                pressureDBack[i][j] = msg.segment[i].command[j].pressure;
            }
        }
    }

    void PIDdataCallback(const origarm_ros::PIDdata &msg)
    {
        int segn = 2;
        transdiffSegmentData.dalphaRatio = msg.dalphaRatio[segn];
        transdiffSegmentData.dbeta = msg.dbeta[segn];
        transdiffSegmentData.dlength = msg.dlength[segn];

        transdiffSegmentData.alphaP = msg.alphaP[segn];
        transdiffSegmentData.alphaI = msg.alphaI[segn];
        transdiffSegmentData.alphaD = msg.alphaD[segn];

        transdiffSegmentData.betaP = msg.betaP[segn];
        transdiffSegmentData.betaI = msg.betaI[segn];
        transdiffSegmentData.betaD = msg.betaD[segn];

        transdiffSegmentData.lengthP = msg.lengthP[segn];
        transdiffSegmentData.lengthI = msg.lengthI[segn];
        transdiffSegmentData.lengthD = msg.lengthD[segn];
    }

    void transFFCallback(const origarm_ros::transFFCompensateData &msg)
    {
        int segn = 2;
        transdiffSegmentData.transFFFlag = msg.transFFCData[segn].transFFFlag;
        transdiffSegmentData.pressureCommandFiltered = msg.transFFCData[segn].FFPressure;
        transdiffSegmentData.transFFTalpha = msg.transFFCData[segn].transFFTalpha;
        transdiffSegmentData.transFFTbeta = msg.transFFCData[segn].transFFTbeta;
        for (int j = 0; j < ACTNUM; j++)
        {
            transdiffSegmentData.pressurefeedforwardTrans[j] = msg.transFFCData[segn].transFFPressure[j];
        }
        transdiffSegmentData.vectorAB_Dx = msg.transFFCData[segn].vectorAB_Dx;
        transdiffSegmentData.vectorAB_Dy = msg.transFFCData[segn].vectorAB_Dy;
        transdiffSegmentData.vectorAB_rx = msg.transFFCData[segn].vectorAB_rx;
        transdiffSegmentData.vectorAB_ry = msg.transFFCData[segn].vectorAB_ry;
        transdiffSegmentData.vectorAB_Errx = msg.transFFCData[segn].vectorAB_Errx;
        transdiffSegmentData.vectorAB_Erry = msg.transFFCData[segn].vectorAB_Erry;
        transdiffSegmentData.vectorAB_ErrProjx = msg.transFFCData[segn].vectorAB_ErrProjx;
        transdiffSegmentData.vectorAB_ErrProjy = msg.transFFCData[segn].vectorAB_ErrProjy;
        transdiffSegmentData.vectorAB_FBx = msg.transFFCData[segn].vectorAB_FBx;
        transdiffSegmentData.vectorAB_FBy = msg.transFFCData[segn].vectorAB_FBy;
    }

    void pub()
    {

        for (int i = 0; i < SEGNUM; i++)
        {
            if (transdiffConfigs.transdiff_Configuration[i].transMode == 2)
            {

                states_.ABL.segment[i].A = alpha_passive[i];
                states_.ABL.segment[i].B = beta_passive[i];
                states_.ABL.segment[i].L = length_passive[i];

                if (i == 2)
                {
                    transdiffSegmentData.alphaPassiveFiltered = ablPassiveFilter[i][0].getValue();
                    transdiffSegmentData.betaPassiveFiltered = ablPassiveFilter[i][1].getValue();
                    transdiffSegmentData.lengthPassiveFiltered = ablPassiveFilter[i][2].getValue();
                }
            }
            else
            {
                states_.ABL.segment[i].A = alpha[i];
                states_.ABL.segment[i].B = beta[i];
                states_.ABL.segment[i].L = lengthAbsM[i];

                if (i == 2)
                {
                    transdiffSegmentData.alphaLaserFiltered = ablStateFilter[i][0].getValue();
                    transdiffSegmentData.betaLaserFiltered = ablStateFilter[i][1].getValue();
                    transdiffSegmentData.lengthLaserFiltered = ablStateFilter[i][2].getValue();
                }
            }
        }

        int segn = 2;

        transdiffSegmentData.transMode = transdiffConfigs.transdiff_Configuration[segn].transMode;
        transdiffSegmentData.transSegment = transdiffConfigs.transdiff_Configuration[segn].transSegment;
        transdiffSegmentData.transActuatorNum = transdiffConfigs.transdiff_Configuration[segn].transActuatorNum;
        transdiffSegmentData.transPassiveNum = transdiffConfigs.transdiff_Configuration[segn].transPassiveNum;

        transdiffSegmentData.initTime0 = initTransTime0[segn];
        transdiffSegmentData.passedDuration = initTransDuration[segn];

        transdiffSegmentData.alphaCommand = ablCommand.segment[segn].A;
        transdiffSegmentData.betaCommand = ablCommand.segment[segn].B;
        transdiffSegmentData.lengthCommand = ablCommand.segment[segn].L;
        transdiffSegmentData.alphaPassive = alpha_passive[segn];
        transdiffSegmentData.betaPassive = beta_passive[segn];
        transdiffSegmentData.lengthPassive = length_passive[segn];
        transdiffSegmentData.alphaLaser = alpha[segn];
        transdiffSegmentData.betaLaser = beta[segn];
        transdiffSegmentData.lengthLaser = lengthAbsM[segn];

        for (int j = 0; j < ACTNUM; j++)
        {
            transdiffSegmentData.transActuatorIndex[j] = transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[j];
            transdiffSegmentData.initPressure[j] = transdiffConfigs.transdiff_Configuration[segn].transInitPressure[j]; //HPa
            transdiffSegmentData.initLength[j] = transdiffConfigs.transdiff_Configuration[segn].transInitLength[j];

            transdiffSegmentData.pressureDecayCoeff[j] = leakageRate[segn][j];
            transdiffSegmentData.initPressureCompensated[j] = initTransPressureComp[segn][j];
            transdiffSegmentData.pressureCommand[j] = Cmd_pressure.segment[segn].command[j].pressure; //HPa
            transdiffSegmentData.pressureCommandType[j] = Cmd_pressure.segment[segn].command[j].valve;
            transdiffSegmentData.pressure[j] = pressureActuator[segn][j]; //HPa
            transdiffSegmentData.lengthFromPressure[j] = (lengthfrompressure[segn][j]) * 1000.0f;
            transdiffSegmentData.lengthFromLaser[j] = lengthActuator[segn][j];

            transdiffSegmentData.pressurefeedforward[j] = pressureDFeed[segn][j]; //HPa
            transdiffSegmentData.pressurefeedback[j] = pressureDBack[segn][j];    //HPa
        }

        // real pose calculated from sensor
        // states_.pose.position.x = ;
        // states_.pose.position.y = ;
        // states_.pose.position.z = ;
        // states_.pose.orientation.w = ;
        // states_.pose.orientation.x = ;
        // states_.pose.orientation.y = ;
        // states_.pose.orientation.z = ;
        pub_.publish(states_);
        pub3_.publish(transdiffSegmentData);
    }

private:
    ros::NodeHandle n_;
    ros::Subscriber sub_;
    ros::Subscriber key_sub_;
    ros::Subscriber trans_sub_;
    ros::Subscriber cmd_sub_;
    ros::Subscriber pcmd_sub_;
    ros::Subscriber pfeedforward_sub_;
    ros::Subscriber pfeedback_sub_;
    ros::Subscriber transffcompcmd_sub;
    ros::Subscriber piddata_sub_;

    ros::Publisher pub_;
    ros::Publisher pub2_;
    ros::Publisher pub3_;
    origarm_ros::States states_;
    origarm_ros::States states_filtered;
    origarm_ros::Command_ABL ablCommand;
    origarm_ros::Command_Pre_Open Cmd_pressure;
    origarm_ros::transdiffDataSeg transdiffSegmentData;
};

/**/
/**
 * @brief Choose good actuators. More complex rules could be added
 *
 * @param p the segment number
 * @param q the actautor number on the segment
 * @return 0: Bad actuators
 *         1: Good actuators
 */
static int goodActuator(int p, int q)
{
    int ret = 0;
    if (p >= 0 && p < SEGNUM && q >= 0 && q < ACTNUM)
        if (goodIMU[p][q])
            ret = 1;
    return ret;
}

/**
 * @brief Init all frames
 * 1. init the IMU body frame at time 0 in the ARM Base, based on the physical location of the actuators in the arm
 * 2. Read the IMU body frame at time 0 in the IMU Base
 * 3. Calculate the transformation matrix of IMU Base in the ARM Base
 * 4. define the plate pose at time 0 in the Arm base, which are all identities.
 */
void InitFrames()
{
    /*set the IMU body frame at time 0 in the ARM Base*/

    for (int i = 0; i < SEGNUM; i++)
    {
        R0InArm[i][0] = myRotz(-M_PI / 2);
        R0InArm[i][2] = myRotz(-M_PI / 2 + 2 * M_PI / 3);
        R0InArm[i][4] = myRotz(-M_PI / 2 + 4 * M_PI / 3);

        R0InArm[i][1] = myRotz(-M_PI / 6) * myRoty(M_PI);
        R0InArm[i][3] = myRotz(-M_PI / 6 + 2 * M_PI / 3) * myRoty(M_PI);
        R0InArm[i][5] = myRotz(-M_PI / 6 + 4 * M_PI / 3) * myRoty(M_PI);
    }

    /*Read the IMU body frame at time 0 in the IMU Base*/
    string path = IMUDataPath + quatReadFileName;
    readQuatFromFile(path, Quat0InIMU);
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            R0InIMU[i][j] = Quat0InIMU[i][j];
        }
    }

#if DUMMY_QUAT_USED == 1
    /*****************Debug Only.  Uncomment in real application***********************/
    /*Read dummy IMU body frame at time t in the IMU Base*/
    readQuatFromFile(IMUDataPath + quattDummyFileName, QuattInIMU);
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            RtInIMU[i][j] = QuattInIMU[i][j];
        }
    }
#endif

    /*Calculate the transformation matrix of IMU Base in the ARM Base*/
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            Rtrans[i][j] = R0InArm[i][j] * R0InIMU[i][j].transpose();
        }
    }

    /*define the plate pose at time 0 in the Arm base*/
    for (int i = 0; i < SEGNUM; i++)
    {
        R0PlateInArm[i] = Matrix3f::Identity();
    }
}

/*get the IMU body frame at time t in the arm base*/
void getRtInArm()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            RtInArm[i][j] = Rtrans[i][j] * RtInIMU[i][j];
        }
    }
}

/**
 * @brief get the IMU body frame change at time t relative to time 0,  in the Arm Base
 *
 */
void getdRtInArm()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            dRInArm[i][j] = RtInArm[i][j] * R0InArm[i][j].transpose();
        }
    }
}

/**
 * @brief get ddRtInArm using different actautor pairs.
 * There are 3*9 pairs for each segment, that is
 * 1. Using Act(1 3 5) in previous segment and Act(2 4 6) in current segment
 * 2. Using Act(0 2 4) and Act(1 3 5) in current segment
 * 3. Using Act(0 2 4) in current segment and Act(0 2 4) in next segment
 * Only Elegible and good Actuators are considered during the process.
 */
void getddRtInArm()
{
    int p1 = 0; //segment number of the first chosen actuator
    int p2 = 0; //segment number of the second chosen actuator
    int k = 0;  //0~8, representing all the 9 possbile actuator combinations in a given flag

    /*For every segment*/
    for (int i = 0; i < SEGNUM; i++)
    {

        /*Using Act(1 3 5) in previous segment and Act(2 4 6) in current segment*/
        if (calculate_alpha_using_pre_cur_flag)
        {
            k = 0;
            p1 = i - 1;
            p2 = i;
            for (int q1 = 1; q1 < ACTNUM; q1 += 2)
            {
                for (int q2 = 1; q2 < ACTNUM; q2 += 2)
                {
                    /*only Elegible and good Actuator*/
                    if (goodActuator(p1, q1) && goodActuator(p2, q2))
                        ddRInArm[k++][0][i] = dRInArm[p1][q1].transpose() * dRInArm[p2][q2];
                }
            }
        }

        /*Using Act(0 2 4) and Act(1 3 5) in current segment*/
        if (calculate_alpha_using_cur_cur_flag)
        {
            k = 0;
            p1 = i;
            p2 = i;
            for (int q1 = 0; q1 < ACTNUM; q1 += 2)
            {
                for (int q2 = 1; q2 < ACTNUM; q2 += 2)
                {
                    /*only Elegible and good Actuator*/
                    if (goodActuator(p1, q1) && goodActuator(p2, q2))
                        ddRInArm[k++][1][i] = dRInArm[p1][q1].transpose() * dRInArm[p2][q2];
                }
            }
        }
        /*Using Act(0 2 4) in current segment and Act(0 2 4) in next segment*/
        if (calculate_alpha_using_cur_nex_flag)
        {
            k = 0;
            p1 = i;
            p2 = i + 1;
            for (int q1 = 0; q1 < ACTNUM; q1 += 2)
            {
                for (int q2 = 0; q2 < ACTNUM; q2 += 2)
                {
                    /*only Elegible and good Actuator*/
                    if (goodActuator(p1, q1) && goodActuator(p2, q2))
                        ddRInArm[k++][2][i] = dRInArm[p1][q1].transpose() * dRInArm[p2][q2];
                }
            }
        }
    }
}

/**
 * @brief Get the Alpha From R object
 *
 * @param R
 * @return float
 */
float getAlphaFromR(Matrix3f &R)
{
    float val = -1000;
    float alphaCandi = 0;
    if (!R.isZero())
    {
        val = CONSTRAIN(R(2, 2), -1, 1);
        alphaCandi = acos(val);
    }
    else
    {
        alphaCandi = -1000;
    }
    return alphaCandi;
}
/**
 * @brief Get the Beta From a rotation matrix R
 *
 * @param R The rotation matrix
 * @return float beta
 */
float getBetaFromR(Matrix3f &R)
{
    float alphaCandi = 0;
    float betaCandi = 0;
    alphaCandi = getAlphaFromR(R);
    if (alphaCandi > measureDeadzoneAlpha || alphaCandi < -measureDeadzoneAlpha)
    {
        betaCandi = atan2(R(1, 2) / sin(alphaCandi), R(0, 2) / sin(alphaCandi));
    }

    else
    {
        betaCandi = 0;
    }
    return betaCandi;
}
/**
 * @brief Get the Alpha Beta From Actuators.
 * For every segment, there are 3*9 = 27 candidate transformation matrix ddR for calculating alpha and beta.
 * The valid values are chosen and averaged to get the final value
 *
 */
void getAlphaBetaFromActuators()
{
    float sumAlpha = 0;
    int countsAlpha = 0;
    float sumBeta = 0;
    float countsBeta = 0;
    int average_flag[3] = {calculate_alpha_using_pre_cur_flag, calculate_alpha_using_cur_cur_flag, calculate_alpha_using_cur_nex_flag};

    for (int i = 0; i < SEGNUM; i++)
    {
        sumAlpha = 0;
        countsAlpha = 0;
        sumBeta = 0;
        countsAlpha = 0;
        for (int j = 0; j < 3; j++)
        {
            if (average_flag[j])
            {
                for (int k = 0; k < 9; k++)
                {
                    alphaCandidates[k][j][i] = getAlphaFromR(ddRInArm[k][j][i]);
                    betaCandidates[k][j][i] = getBetaFromR(ddRInArm[k][j][i]);
                    /*unuseful data are set to be -1000*/
                    if (alphaCandidates[k][j][i] > -10)
                    {
                        sumAlpha += alphaCandidates[k][j][i];
                        countsAlpha++;
                        sumBeta += betaCandidates[k][j][i];
                        countsBeta++;
                    }
                }
            }
        }
        alphaFromActuators[i] = sumAlpha / countsAlpha;
        betaFromActuators[i] = sumBeta / countsBeta;
    }
}

/**
 * @brief Average quaternions in an array Quats[], starting from index, with a total number of startnum
 *
 * @param Quats The quaternion array to be averaged
 * @param startnum The starting index
 * @param num Number of total quaternion to be averaged
 * @return Quaternionf Averaged quaternion
 */
Quaternionf averageQuaternions(Quaternionf *Quats, int startnum, int num)
{
    Quaternionf avQuat;
    Eigen::MatrixXf Qori(4, num);
    Eigen::Matrix4f Qmat;
    Eigen::Vector4f QmatEigenValue;
    Eigen::Matrix4f QmatEigenVector;
    SelfAdjointEigenSolver<Matrix4f> es(Qmat);
    float coeff = 1.0f / num;
    for (int i = 0; i < num; i++)
    {
        Qori(0, i) = Quats[startnum + i].w() * coeff;
        Qori(1, i) = Quats[startnum + i].x() * coeff;
        Qori(2, i) = Quats[startnum + i].y() * coeff;
        Qori(3, i) = Quats[startnum + i].z() * coeff;
    }
    Qmat = Qori * Qori.transpose();
    QmatEigenValue = es.eigenvalues();
    QmatEigenVector = es.eigenvectors();
    avQuat.w() = QmatEigenVector(0, 3);
    avQuat.x() = QmatEigenVector(1, 3);
    avQuat.y() = QmatEigenVector(2, 3);
    avQuat.z() = QmatEigenVector(3, 3);
    return avQuat;
}

/**
 * @brief Get the Plates rotation matrix R in the Arm base
 * The plates quaternions are determined by averaging the quaternions of the actuators on it. Physically these actuator share a common quaternion
 * The choice of Actuators can choose the three actuators on current segment or the next segment.
 */
void getPlatesR()
{
    int use_cur_seg = 1;
    int use_nex_seg = 1;
    Quaternionf quaternionCandi[6];

    int startnum = 0;
    int num = 0;
    for (int i = 0; i < SEGNUM; i++)
    {

        /*The last plate could only use current segment actuators*/
        if (use_cur_seg == 1 || i == SEGNUM - 1)
        {
            quaternionCandi[0] = dRInArm[i][1];
            quaternionCandi[1] = dRInArm[i][3];
            quaternionCandi[2] = dRInArm[i][5];
            startnum = 0;
            num = 3;
        }
        if (use_nex_seg == 1 && (i != SEGNUM - 1))
        {
            quaternionCandi[3] = dRInArm[i + 1][0];
            quaternionCandi[4] = dRInArm[i + 1][2];
            quaternionCandi[5] = dRInArm[i + 1][4];
            startnum += 3;
            num += 3;
        }
        quaternionPlates[i] = averageQuaternions(quaternionCandi, startnum, num);
        RtPlateInArm[i] = quaternionPlates[i];
    }
}

/**
 * @brief Get the Alpha Beta From the relative rotation matrix between plates
 *
 */
void getAlphaBetaFromPlates()
{
    Matrix3f dRplate;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i == 0)
            dRplate = RtPlateInArm[i];
        else
            dRplate = RtPlateInArm[i - 1].transpose() * RtPlateInArm[i];
        alphaFromPlates[i] = getAlphaFromR(dRplate);
        betaFromPlates[i] = getBetaFromR(dRplate);
    }
}

/**
 * @brief Get the Alpha of all the segments
 *
 */
static void getAlpha(float (&alphaCandi)[SEGNUM])
{
    for (int i = 0; i < SEGNUM; i++)
    {
        alpha[i] = alphaCandi[i];
        alphaDeg[i] = alpha[i] * rad2deg;
    }
}

/**
 * @brief Get the Beta af all the segments
 *
 */
static void getBeta(float (&betaCandi)[SEGNUM])
{
    for (int i = 0; i < SEGNUM; i++)
    {
        beta[i] = betaCandi[i];
        betaDeg[i] = betaCandi[i] * rad2deg;
    }
}

/**
 * @brief Get the Beta af all the segments
 *
 */
// static void getBetaFromLength(float (&lengthAbsMM)[SEGNUM])
// {
//     for (int i = 0; i < SEGNUM; i++)
//     {
//         beta[i] = betaCandi[i];
//         betaDeg[i] = betaCandi[i] * rad2deg;
//     }
// }

static void getLength()
{
    float lengthSum = 0;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i > 5)
        {
            lengthAbsMM[i] = g_length0MM;
            lengthAbsM[i] = g_length0;
        }
        else
        {
            lengthSum = 0;
            for (int j = 0; j < ACTNUM; j++)
            {
                lengthSum += lengthActuator[i][j];
            }

            lengthAbsMM[i] = lengthSum / ACTNUM + g_length0MM;
            lengthAbsM[i] = lengthAbsMM[i] / 1000.0f;
        }
    }
}

/**
 * @brief Estmate the alpha, beta, lengthAbsM from IMU data for all the segments
 *
 */
void estimateABL()
{
    /*Calculation of useful matrices*/
    getRtInArm();
    getdRtInArm();
    getddRtInArm();

    /*Two methods to obtain alpha and beta*/
    getAlphaBetaFromActuators();
    getAlphaBetaFromPlates();

    /*Choose a result for alpha and beta*/
    getAlpha(alphaFromActuators);
    getBeta(betaFromActuators);

    /*estimate length*/
    getLength();
}

static void getArcLength(float (&rotangle)[SEGNUM])
{
    float lengthSum, Length1, Length2, lambda, arcLength;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i > 5)
        {
            lengthAbsMM[i] = g_length0MM;
            lengthAbsM[i] = g_length0;
        }
        else
        {
            lengthSum = 0;
            for (int j = 0; j < ACTNUM; j++)
            {
                lengthSum += lengthActuator[i][j];
            }

            if (rotangle[i] == 0)
            {
                lengthAbsMM[i] = lengthSum / ACTNUM + g_length0MM;
                lengthAbsM[i] = lengthAbsMM[i] / 1000.0f;
            }
            else
            {
                Length1 = (lengthActuator[i][0] + lengthActuator[i][2] + lengthActuator[i][4]) / 3;
                Length2 = (lengthActuator[i][1] + lengthActuator[i][3] + lengthActuator[i][5]) / 3;
                lambda = (Length1 - Length2 * cos(rotangle[i])) / (1 - cos(rotangle[i]) * cos(rotangle[i]));
                arcLength = (lambda * cos(rotangle[i]) * rotangle[i]) / (2 * sin(rotangle[i] / 2));
                lengthAbsMM[i] = arcLength + g_length0MM;
                lengthAbsM[i] = arcLength / 1000.0f;
            }
        }
    }
}

Eigen::Vector3f getNormalVectfromCovMatrix(Eigen::Matrix3f &CovM)
{
    Matrix3f U, S, V;
    Vector3f normalV;
    JacobiSVD<Eigen::Matrix3f> svd(CovM, ComputeFullU | ComputeFullV);

    static Eigen::IOFormat dispMatFormat(4, 0, ", ", "\n", "[", "]");
    static std::string separator = "\n---------------------------------------\n";

    U = svd.matrixU();
    V = svd.matrixV();

    // normal direction of plate should point towards +z
    if (V(2, 2) < 0)
    {
        normalV(0, 0) = -V(0, 2);
        normalV(1, 0) = -V(1, 2);
        normalV(2, 0) = -V(2, 2);
    }
    else
    {
        normalV(0, 0) = V(0, 2);
        normalV(1, 0) = V(1, 2);
        normalV(2, 0) = V(2, 2);
    }

    return normalV;
}

Eigen::Matrix3f getdRplatefromNormalVect(Eigen::Vector3f &Vn)
{
    Vector3f V0, crossV;
    Matrix3f dRplate;
    float rotangle;

    V0 << 0,
        0,
        1;

    if ((Vn - V0).isMuchSmallerThan(1e-3))
    {
        crossV = V0;
    }
    else
    {
        crossV = V0.cross(Vn);
    }

    rotangle = acos(Vn.dot(V0));
    dRplate = AngleAxisf(rotangle, crossV);

    return dRplate;
}

void getAlphaBetafromLaser()
{
    MatrixXf A(ACTNUM, 3);
    Matrix3f Q, dRplate;
    Vector3f NormalVect;
    float Zc[SEGNUM], z[SEGNUM][ACTNUM];
    float Zsum;

    NormalVect << 0,
        0,
        1;

    // compute middle point
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i > 5)
        {
            alphaFromPlates[i] = 0;
            betaFromPlates[i] = 0;
        }
        else
        {
            Zsum = 0;
            for (int j = 0; j < ACTNUM; j++)
            {
                Zsum += lengthActuator[i][j];
                z[i][j] = lengthActuator[i][j];
            }

            Zc[i] = Zsum / ACTNUM;

            // printf("Xc[%d]: %f, Yc[%d]: %f, Zc[%d]: %f\n",i, Xc[i], i, Yc[i], i, Zc[i]);

            A(0, 0) = cos(0 * M_PI / 3) * g_install_radiusMM;
            A(0, 1) = sin(0 * M_PI / 3) * g_install_radiusMM;
            A(0, 2) = z[i][0] - Zc[i];
            A(1, 0) = cos(1 * M_PI / 3) * g_install_radiusMM;
            A(1, 1) = sin(1 * M_PI / 3) * g_install_radiusMM;
            A(1, 2) = z[i][1] - Zc[i];
            A(2, 0) = cos(2 * M_PI / 3) * g_install_radiusMM;
            A(2, 1) = sin(2 * M_PI / 3) * g_install_radiusMM;
            A(2, 2) = z[i][2] - Zc[i];
            A(3, 0) = cos(3 * M_PI / 3) * g_install_radiusMM;
            A(3, 1) = sin(3 * M_PI / 3) * g_install_radiusMM;
            A(3, 2) = z[i][3] - Zc[i];
            A(4, 0) = cos(4 * M_PI / 3) * g_install_radiusMM;
            A(4, 1) = sin(4 * M_PI / 3) * g_install_radiusMM;
            A(4, 2) = z[i][4] - Zc[i];
            A(5, 0) = cos(5 * M_PI / 3) * g_install_radiusMM;
            A(5, 1) = sin(5 * M_PI / 3) * g_install_radiusMM;
            A(5, 2) = z[i][5] - Zc[i];

            // covariance matrix Q, to minimize |QX|, where X is normal vector of plate
            Q = (A.transpose() * A) / ACTNUM;

            NormalVect = getNormalVectfromCovMatrix(Q);
            dRplate = getdRplatefromNormalVect(NormalVect);
            alphaFromPlates[i] = getAlphaFromR(dRplate);
            betaFromPlates[i] = getBetaFromR(dRplate);

            // printf("alphaFromLength[%d]: %f\n", i, alphaFromPlates[i]);
            // printf("betaFromLength[%d]: %f\n", i, betaFromPlates[i]);
        }
    }
}

void getABLfromLaser()
{
    /*estimate alpha & beta*/
    getAlphaBetafromLaser();
    getAlpha(alphaFromPlates);
    getBeta(betaFromPlates);

    /*estimate length*/
    getLength();
    // getArcLength(alphaFromPlates);
}

/**
 * @brief Get the Gauge Pressure With Leakage consideration
 *
 * @param p0 The initial gauge pressure of the closed actuator
 * @param elapsedT passsing time (seconds)
 * @param lrate leakage rate, either positive or negative value is OK.
 * @return float
 */
float getGaugePressureWithLeakage(float p0, float elapsedT, float lrate)
{
    elapsedT = abs(elapsedT);
    lrate = -abs(lrate);
    return p0 * exp(lrate * elapsedT);
}

/**
 * @brief Get the Compensated InitTransPressure for certain actuator considering the leakage
 *
 * @param segn segment number
 * @param actn actuator number
 * @return float leakage compensated pressure
 */
float getCompensatedInitTransPressure(int segn, int actn)
{
    ros::Time curT = ros::Time::now();
    initTransDuration[segn] = curT - initTransTime0[segn];
    float passt = initTransDuration[segn].toSec();
    return getGaugePressureWithLeakage(transdiffConfigs.transdiff_Configuration[segn].transInitPressure[actn], passt, leakageRate[segn][actn]); //gauge hpa
}

/**
 * @brief Get the Length From Pressure object within a closed chamber
 *        Based on PV=nRT, the nominal equation is P0*(A*L0 + deadV) =Pt*(A*Lt + deadV), and the result is  Lt=(P0/Pt)*(L0 + d/A)-(d/A).
 *        If the leakage of the chamber is considered, then the equation should be modified as a time-decaying P0t*(A*L0 + deadV) =Pt*(A*Lt + deadV).
 *        P0t could be calculated from the The leakage rate k which is measured by experiments.
 *        Both P and L shoule be changed to absolute value
 * @param segn
 * @param actn
 * @return float relative lengthRelM in relative m
 */
float getLengthFromPressure(int segn, int actn)
{

    static float deadVolume = M_PI * 0.00125 * 0.00125 * 2 * 1.5; //4mm tube (2.5mm inner diameter, 1.5m length)

    float Aequal = g_actuator_effectiveArea * 1.2;
    float L0equal = g_length0 * 1.1;
    float deadVolumeOverArea = deadVolume / Aequal;

    initTransPressureComp[segn][actn] = getCompensatedInitTransPressure(segn, actn);
    float dLFromZero = ((initTransPressureComp[segn][actn] * 100 + 101325) / (pressureActuator[segn][actn] * 100 + 101325) * ((transdiffConfigs.transdiff_Configuration[segn].transInitLength[actn] / 1000.0f + L0equal) + deadVolumeOverArea) - deadVolumeOverArea) - L0equal;
    float dLFromInit = dLFromZero-transdiffConfigs.transdiff_Configuration[segn].transInitLength[actn] / 1000.0f;
    if (actn == 2 || actn == 4)
    {
        dLFromInit = dLFromInit * 1.5;
    }
    dLFromZero=dLFromInit+transdiffConfigs.transdiff_Configuration[segn].transInitLength[actn] / 1000.0f;
    return dLFromZero;
}

Vector3f getNormalVecFromPoints(const Ref<const MatrixXf> &pointsMat, int pointsNum)
{
    static Matrix3f Q;
    static Vector3f Vn;
    Q = (pointsMat.transpose() * pointsMat) / pointsNum;
    Vn = getNormalVectfromCovMatrix(Q);
    return Vn;
}

/**
 * @brief Get Alpha Beta Length From Points
 *
 * @param pointsMat the points matrix with dimension(pointsNum,3), with zero mean in Z axis
 * @param pointsNum total number of points
 * @param optBeta optional supplymentary beta information
 * @param pointsInd index of two points
 * @return Vector3f
 */
Vector3f getStatesFromPoints(Ref<MatrixXf> pointsMat, int pointsNum, float optBeta, int *pointsInd)
{

    Vector3f Vnorm, Vret;
    static Matrix3f dRplateLocal;
    float lengthMean;

    if (pointsNum == 2)
    {
        float oa1 = abs(cos(g_install_angles[pointsInd[0]] - optBeta) * g_install_radius);
        float ob1 = abs(-cos(g_install_angles[pointsInd[1]] - optBeta) * g_install_radius);
        float length1 = pointsMat(0, 2);
        float length2 = pointsMat(1, 2);
        float d = 0;
        float tanAlpha = 0;
        float al;
        float len;

        if (length1 != length2)
        {
            d = abs((length2 * (oa1 + ob1) / (length1 - length2)));
            tanAlpha = abs(length2 / d);
            al = atan(tanAlpha);
            if (length1 > length2)
            {
                len = tanAlpha * (d + ob1);
            }
            else
            {
                len = tanAlpha * (d - ob1);
            }
        }
        else
        {
            len = length1;
            al = 0;
        }

        printf("length1:%3.3f, length2:%3.3f d=%3.3f tanAlpha=%3.3f alpha=%3.3f len=%3.3f optBeta=%3.3f \n",
               length1,
               length2,
               d,
               tanAlpha,
               al,
               len,
               optBeta);

        Vret(0) = al;
        Vret(1) = optBeta;
        Vret(2) = len;
    }
    else
    {
        //length
        lengthMean = pointsMat.col(2).mean();

        Vret(2) = lengthMean; //absolute M
        // cout << "pointsMat" << pointsMat << endl;
        // cout << "pointsMatCol(2)=" << pointsMat.col(2) << endl;
        // cout << "pointsMatCol(2).mean()=" << lengthMean << endl;

        //zero mean of Z axis for the points matrix
        for (int i = 0; i < pointsNum; i++)
        {
            pointsMat(i, 2) -= lengthMean;
        }

        Vnorm = getNormalVecFromPoints(pointsMat, pointsNum);
        // cout << "Vnorm=" << Vnorm << endl;
        dRplateLocal = getdRplatefromNormalVect(Vnorm);
        // cout << "Rplate=" << dRplateLocal << endl;
        //alpha
        Vret(0) = getAlphaFromR(dRplateLocal);

        //beta
        Vret(1) = getBetaFromR(dRplateLocal);

        // cout << "Vret=" << Vret << endl;
    }

    return Vret;
}

/**
 * @brief Estimate the alpha, beta, length from pressure data for certain segments
 *
 */
static Vector3f passiveSensingOneSeg(int segn)
{
    static Matrix3f Q, dRplate;

    int actuatorNum = transdiffConfigs.transdiff_Configuration[segn].transActuatorNum;
    int passiveActNum = transdiffConfigs.transdiff_Configuration[segn].transPassiveNum;

    MatrixXf A = MatrixXf::Zero(passiveActNum, 3);
    float lengthsum, lengthc[SEGNUM];

    Vector3f statePassive;

    int passiveActIndex[ACTNUM] = {0, 0, 0, 0, 0, 0};

    //update local PassiveIndexBuffer
    for (int i = 0; i < passiveActNum; i++)
    {
        passiveActIndex[i] = transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[i + actuatorNum];
    }

    for (int i = 0; i < passiveActNum; i++)
    {
        lengthfrompressure[segn][passiveActIndex[i]] = getLengthFromPressure(segn, passiveActIndex[i]);
    }

    //points coordinates
    for (int i = 0; i < passiveActNum; i++)
    {
        A(i, 0) = cos(g_install_angles[passiveActIndex[i]]) * g_install_radius;
        A(i, 1) = sin(g_install_angles[passiveActIndex[i]]) * g_install_radius;
        A(i, 2) = lengthfrompressure[segn][passiveActIndex[i]] + g_length0;
    }

    if (passiveActNum >= 3)
    {
        // cout << "A=" << A << endl;
        // cout << "passiveActNum" << passiveActNum << endl;
        // printf("passiveActIndex %d %d %d %d %d %d\n", passiveActIndex[0], passiveActIndex[1], passiveActIndex[2], passiveActIndex[3], passiveActIndex[4], passiveActIndex[5]);

        statePassive = getStatesFromPoints(A, passiveActNum, 0, passiveActIndex);
    }
    else
    {
        float deducedBeta = 0;
        float pcos = 0;
        float psin = 0;
        for (int i = 0; i < transdiffConfigs.transdiff_Configuration[segn].transActuatorNum; i++)
        {
            int ind = transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[i];
            pcos += pressureActuator[segn][ind] * cos(g_install_angles[ind]);
            psin += pressureActuator[segn][ind] * sin(g_install_angles[ind]);
        }
        deducedBeta = atan2(psin, pcos) - M_PI;
        deducedBeta = 0;
        statePassive = getStatesFromPoints(A, passiveActNum, deducedBeta, passiveActIndex);
    }
    return statePassive;
}

void transSegmentsPassiveSensing()
{
    // for (int i = 0; i < SEGNUM; i++)
    // {
    //     printf("transdiffConfigs in StateEstimator, Seg %d, Mode %d, ActNum %d, PassiveNum %d, ActIndex %d %d %d %d %d %d\n",
    //            transdiffConfigs.transdiff_Configuration[i].transSegment,
    //            transdiffConfigs.transdiff_Configuration[i].transMode,
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorNum,
    //            transdiffConfigs.transdiff_Configuration[i].transPassiveNum,
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[0],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[1],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[2],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[3],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[4],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[5]);
    // }

    Vector3f statePassive;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i < 6)
        {
            if (transdiffConfigs.transdiff_Configuration[i].transMode == 2)
            {
                statePassive = passiveSensingOneSeg(i);
                alpha_passive[i] = statePassive(0);
                beta_passive[i] = statePassive(1);
                length_passive[i] = statePassive(2);
            }
        }
    }
}

void initTransParameters()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        transdiffConfigs.transdiff_Configuration[i].transMode = 0;
        transdiffConfigs.transdiff_Configuration[i].transSegment = i;
        transdiffConfigs.transdiff_Configuration[i].transActuatorNum = 6;
        transdiffConfigs.transdiff_Configuration[i].transPassiveNum = 0;
        for (int j = 0; j < ACTNUM; j++)
        {
            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[j] = j;
        }
    }
}

void filterLaserState()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        ablStateFilter[i][0].update(alpha[i]);
        ablStateFilter[i][1].update(beta[i]);
        ablStateFilter[i][2].update(lengthAbsM[i]);
        ablPassiveFilter[i][0].update(alpha_passive[i]);
        ablPassiveFilter[i][1].update(beta_passive[i]);
        ablPassiveFilter[i][2].update(length_passive[i]);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "State_Estimator_Node");

    IMUDataPath = ros::package::getPath("origarm_ros") + "/predefined_param/";
    cout << "IMUDataPath:" << IMUDataPath << endl;

    InitFrames();
    initTransParameters();
    initFilters();
    State_Estimator stateEstimator;

    ros::AsyncSpinner s(0);

    s.start();

    ros::Rate loop_rate(100);

    ROS_INFO("Ready for State_Estimator_Node");

    int printFre = 0;
    // BeginTime = ros::Time::now();
    while (ros::ok())
    {
        //estimateABL();
        getABLfromLaser();

        transSegmentsPassiveSensing();

        filterLaserState();

        stateEstimator.pub();

        // if (printFre++ > 2)
        // {
        //     printf("A[0]:%.3f, [1]:%.3f, [2]:%.3f, [3]:%.3f, [4]:%.3f, [5]:%.3f  | B[0]:%.3f, [1]:%.3f, [2]:%.3f, [3]:%.3f, [4]:%.3f, [5]:%.3f  |  L[0]:%d, [1]:%d, [2]:%d, [3]:%d, [4]:%d, [5]:%d\n",
        //            (alpha[0]),
        //            (alpha[1]),
        //            (alpha[2]),
        //            (alpha[3]),
        //            (alpha[4]),
        //            (alpha[5]),
        //            (beta[0]),
        //            (beta[1]),
        //            (beta[2]),
        //            (beta[3]),
        //            (beta[4]),
        //            (beta[5]),
        //            (int)(lengthAbsMM[0]),
        //            (int)(lengthAbsMM[1]),
        //            (int)(lengthAbsMM[2]),
        //            (int)(lengthAbsMM[3]),
        //            (int)(lengthAbsMM[4]),
        //            (int)(lengthAbsMM[5]));
        //     printFre = 0;
        // }

        // if (printFre++ > 2)
        // {
        //     printf("Time:%s\r\n", getTimeString().c_str());
        //     printf("ABLfromLaser        | ABLfromPassiveSensing        | LengthfromPressure\r\n");
        //     for (int i = 0; i < 6; i++)
        //     {
        //         printf("Data[%d]: %.3f %.3f %.3f |%.3f %.3f %.3f |%.3f %.3f %.3f %.3f %.3f %.3f\r\n", i,
        //                alpha[i], beta[i], lengthAbsM[i],
        //                alpha_passive[i], beta_passive[i], length_passive[i],
        //                lengthfrompressure[i][0],
        //                lengthfrompressure[i][1],
        //                lengthfrompressure[i][2],
        //                lengthfrompressure[i][3],
        //                lengthfrompressure[i][4],
        //                lengthfrompressure[i][5]);
        //     }
        //     printFre = 0;
        // }

#if DEBUG_DISPLAY == 1
        displayAllMatrix();
#endif
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

max_forks_repo_name: XiaojiaoChen/softArmROS


text:
//==================================================================================================
/*
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================

#include <boost/simd/pack.hpp>
#include <boost/align/aligned_alloc.hpp>
#include <numeric>
#include <simd_test.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  namespace ba = boost::alignment;
  using pack_t = boost::simd::pack<T, N>;

  std::array<T, N> data;
  data.fill(N); // Use the cardinal as scalar value

  pack_t p;

  p = N;
  STF_EXPECT( std::equal(p.begin(), p.end(), data.begin()) );
}

STF_CASE_TPL("Check pack assignment operator with scalar value" , STF_NUMERIC_TYPES)
{
  test<T,  2>(runtime);
  test<T,  4>(runtime);
  test<T,  8>(runtime);
  test<T, 16>(runtime);
  test<T, 32>(runtime);
}


text:
// Copyright (C) 2019-2020 Zilliz. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance
// with the License. You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software distributed under the License
// is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
// or implied. See the License for the specific language governing permissions and limitations under the License

#include <gtest/gtest.h>
#include "query/deprecated/ParserDeprecated.h"
#include "query/Expr.h"
#include "query/PlanNode.h"
#include "query/generated/ExprVisitor.h"
#include "query/generated/PlanNodeVisitor.h"
#include "test_utils/DataGen.h"
#include "query/generated/ShowPlanNodeVisitor.h"
#include "query/generated/ExecExprVisitor.h"
#include "query/Plan.h"
#include "utils/tools.h"
#include <regex>
#include <boost/format.hpp>
#include "segcore/SegmentGrowingImpl.h"
using namespace milvus;

TEST(Expr, Naive) {
    SUCCEED();
    using namespace milvus::wtf;
    std::string dsl_string = R"(
{
    "bool": {
        "must": [
            {
                "term": {
                    "A": [
                        1,
                        2,
                        5
                    ]
                }
            },
            {
                "range": {
                    "B": {
                        "GT": 1,
                        "LT": 100
                    }
                }
            },
            {
                "vector": {
                    "Vec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10
                    }
                }
            }
        ]
    }
})";
}

TEST(Expr, Range) {
    SUCCEED();
    using namespace milvus;
    using namespace milvus::query;
    using namespace milvus::segcore;
    std::string dsl_string = R"(
{
    "bool": {
        "must": [
            {
                "range": {
                    "age": {
                        "GT": 1,
                        "LT": 100
                    }
                }
            },
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10,
                        "round_decimal": 3
                    }
                }
            }
        ]
    }
})";
    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    schema->AddDebugField("age", DataType::INT32);
    auto plan = CreatePlan(*schema, dsl_string);
    ShowPlanNodeVisitor shower;
    Assert(plan->tag2field_.at("$0") == schema->get_offset(FieldName("fakevec")));
    auto out = shower.call_child(*plan->plan_node_);
    std::cout << out.dump(4);
}

TEST(Expr, RangeBinary) {
    SUCCEED();
    using namespace milvus;
    using namespace milvus::query;
    using namespace milvus::segcore;
    std::string dsl_string = R"(
{
    "bool": {
        "must": [
            {
                "range": {
                    "age": {
                        "GT": 1,
                        "LT": 100
                    }
                }
            },
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "Jaccard",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10,
                        "round_decimal": 3
                    }
                }
            }
        ]
    }
})";
    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_BINARY, 512, MetricType::METRIC_Jaccard);
    schema->AddDebugField("age", DataType::INT32);
    auto plan = CreatePlan(*schema, dsl_string);
    ShowPlanNodeVisitor shower;
    Assert(plan->tag2field_.at("$0") == schema->get_offset(FieldName("fakevec")));
    auto out = shower.call_child(*plan->plan_node_);
    std::cout << out.dump(4);
}

TEST(Expr, InvalidRange) {
    SUCCEED();
    using namespace milvus;
    using namespace milvus::query;
    using namespace milvus::segcore;
    std::string dsl_string = R"(
{
    "bool": {
        "must": [
            {
                "range": {
                    "age": {
                        "GT": 1,
                        "LT": "100"
                    }
                }
            },
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10
                    }
                }
            }
        ]
    }
})";
    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    schema->AddDebugField("age", DataType::INT32);
    ASSERT_ANY_THROW(CreatePlan(*schema, dsl_string));
}

TEST(Expr, InvalidDSL) {
    SUCCEED();
    using namespace milvus;
    using namespace milvus::query;
    using namespace milvus::segcore;
    std::string dsl_string = R"(
{
    "float": {
        "must": [
            {
                "range": {
                    "age": {
                        "GT": 1,
                        "LT": 100
                    }
                }
            },
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10
                    }
                }
            }
        ]
    }
})";

    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    schema->AddDebugField("age", DataType::INT32);
    ASSERT_ANY_THROW(CreatePlan(*schema, dsl_string));
}

TEST(Expr, ShowExecutor) {
    using namespace milvus::query;
    using namespace milvus::segcore;
    auto node = std::make_unique<FloatVectorANNS>();
    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    int64_t num_queries = 100L;
    auto raw_data = DataGen(schema, num_queries);
    auto& info = node->search_info_;

    info.metric_type_ = MetricType::METRIC_L2;
    info.topk_ = 20;
    info.field_offset_ = FieldOffset(0);
    node->predicate_ = std::nullopt;
    ShowPlanNodeVisitor show_visitor;
    PlanNodePtr base(node.release());
    auto res = show_visitor.call_child(*base);
    auto dup = res;
    dup["data"] = "...collased...";
    std::cout << dup.dump(4);
}

TEST(Expr, TestRange) {
    using namespace milvus::query;
    using namespace milvus::segcore;
    std::vector<std::tuple<std::string, std::function<bool(int)>>> testcases = {
        {R"("GT": 2000, "LT": 3000)", [](int v) { return 2000 < v && v < 3000; }},
        {R"("GE": 2000, "LT": 3000)", [](int v) { return 2000 <= v && v < 3000; }},
        {R"("GT": 2000, "LE": 3000)", [](int v) { return 2000 < v && v <= 3000; }},
        {R"("GE": 2000, "LE": 3000)", [](int v) { return 2000 <= v && v <= 3000; }},
        {R"("GE": 2000)", [](int v) { return v >= 2000; }},
        {R"("GT": 2000)", [](int v) { return v > 2000; }},
        {R"("LE": 2000)", [](int v) { return v <= 2000; }},
        {R"("LT": 2000)", [](int v) { return v < 2000; }},
        {R"("EQ": 2000)", [](int v) { return v == 2000; }},
        {R"("NE": 2000)", [](int v) { return v != 2000; }},
    };

    std::string dsl_string_tmp = R"(
{
    "bool": {
        "must": [
            {
                "range": {
                    "age": {
                        @@@@
                    }
                }
            },
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10,
                        "round_decimal": 3
                    }
                }
            }
        ]
    }
})";
    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    schema->AddDebugField("age", DataType::INT32);

    auto seg = CreateGrowingSegment(schema);
    int N = 1000;
    std::vector<int> age_col;
    int num_iters = 100;
    for (int iter = 0; iter < num_iters; ++iter) {
        auto raw_data = DataGen(schema, N, iter);
        auto new_age_col = raw_data.get_col<int>(1);
        age_col.insert(age_col.end(), new_age_col.begin(), new_age_col.end());
        seg->PreInsert(N);
        seg->Insert(iter * N, N, raw_data.row_ids_.data(), raw_data.timestamps_.data(), raw_data.raw_);
    }

    auto seg_promote = dynamic_cast<SegmentGrowingImpl*>(seg.get());
    ExecExprVisitor visitor(*seg_promote, seg_promote->get_row_count(), MAX_TIMESTAMP);
    for (auto [clause, ref_func] : testcases) {
        auto loc = dsl_string_tmp.find("@@@@");
        auto dsl_string = dsl_string_tmp;
        dsl_string.replace(loc, 4, clause);
        auto plan = CreatePlan(*schema, dsl_string);
        auto final = visitor.call_child(*plan->plan_node_->predicate_.value());
        EXPECT_EQ(final.size(), N * num_iters);

        for (int i = 0; i < N * num_iters; ++i) {
            auto ans = final[i];

            auto val = age_col[i];
            auto ref = ref_func(val);
            ASSERT_EQ(ans, ref) << clause << "@" << i << "!!" << val;
        }
    }
}

TEST(Expr, TestTerm) {
    using namespace milvus::query;
    using namespace milvus::segcore;
    auto vec_2k_3k = [] {
        std::string buf = "[";
        for (int i = 2000; i < 3000 - 1; ++i) {
            buf += std::to_string(i) + ", ";
        }
        buf += std::to_string(2999) + "]";
        return buf;
    }();

    std::vector<std::tuple<std::string, std::function<bool(int)>>> testcases = {
        {R"([2000, 3000])", [](int v) { return v == 2000 || v == 3000; }},
        {R"([2000])", [](int v) { return v == 2000; }},
        {R"([3000])", [](int v) { return v == 3000; }},
        {R"([])", [](int v) { return false; }},
        {vec_2k_3k, [](int v) { return 2000 <= v && v < 3000; }},
    };

    std::string dsl_string_tmp = R"(
{
    "bool": {
        "must": [
            {
                "term": {
                    "age": {
                        "values": @@@@
                    }
                }
            },
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10,
                        "round_decimal": 3
                    }
                }
            }
        ]
    }
})";
    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    schema->AddDebugField("age", DataType::INT32);

    auto seg = CreateGrowingSegment(schema);
    int N = 1000;
    std::vector<int> age_col;
    int num_iters = 100;
    for (int iter = 0; iter < num_iters; ++iter) {
        auto raw_data = DataGen(schema, N, iter);
        auto new_age_col = raw_data.get_col<int>(1);
        age_col.insert(age_col.end(), new_age_col.begin(), new_age_col.end());
        seg->PreInsert(N);
        seg->Insert(iter * N, N, raw_data.row_ids_.data(), raw_data.timestamps_.data(), raw_data.raw_);
    }

    auto seg_promote = dynamic_cast<SegmentGrowingImpl*>(seg.get());
    ExecExprVisitor visitor(*seg_promote, seg_promote->get_row_count(), MAX_TIMESTAMP);
    for (auto [clause, ref_func] : testcases) {
        auto loc = dsl_string_tmp.find("@@@@");
        auto dsl_string = dsl_string_tmp;
        dsl_string.replace(loc, 4, clause);
        auto plan = CreatePlan(*schema, dsl_string);
        auto final = visitor.call_child(*plan->plan_node_->predicate_.value());
        EXPECT_EQ(final.size(), N * num_iters);

        for (int i = 0; i < N * num_iters; ++i) {
            auto ans = final[i];

            auto val = age_col[i];
            auto ref = ref_func(val);
            ASSERT_EQ(ans, ref) << clause << "@" << i << "!!" << val;
        }
    }
}

TEST(Expr, TestSimpleDsl) {
    using namespace milvus::query;
    using namespace milvus::segcore;

    auto vec_dsl = Json::parse(R"(
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10,
                        "round_decimal": 3
                    }
                }
            }
)");

    int N = 32;
    auto get_item = [&](int base, int bit = 1) {
        std::vector<int> terms;
        // note: random gen range is [0, 2N)
        for (int i = 0; i < N * 2; ++i) {
            if (((i >> base) & 0x1) == bit) {
                terms.push_back(i);
            }
        }
        Json s;
        s["term"]["age"]["values"] = terms;
        return s;
    };
    // std::cout << get_item(0).dump(-2);
    // std::cout << vec_dsl.dump(-2);
    std::vector<std::tuple<Json, std::function<bool(int)>>> testcases;
    {
        Json dsl;
        dsl["must"] = Json::array({vec_dsl, get_item(0), get_item(1), get_item(2, 0), get_item(3)});
        testcases.emplace_back(dsl, [](int x) { return (x & 0b1111) == 0b1011; });
    }

    {
        Json dsl;
        Json sub_dsl;
        sub_dsl["must"] = Json::array({get_item(0), get_item(1), get_item(2, 0), get_item(3)});
        dsl["must"] = Json::array({sub_dsl, vec_dsl});
        testcases.emplace_back(dsl, [](int x) { return (x & 0b1111) == 0b1011; });
    }

    {
        Json dsl;
        Json sub_dsl;
        sub_dsl["should"] = Json::array({get_item(0), get_item(1), get_item(2, 0), get_item(3)});
        dsl["must"] = Json::array({sub_dsl, vec_dsl});
        testcases.emplace_back(dsl, [](int x) { return !!((x & 0b1111) ^ 0b0100); });
    }

    {
        Json dsl;
        Json sub_dsl;
        sub_dsl["must_not"] = Json::array({get_item(0), get_item(1), get_item(2, 0), get_item(3)});
        dsl["must"] = Json::array({sub_dsl, vec_dsl});
        testcases.emplace_back(dsl, [](int x) { return (x & 0b1111) != 0b1011; });
    }

    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    schema->AddDebugField("age", DataType::INT32);

    auto seg = CreateGrowingSegment(schema);
    std::vector<int> age_col;
    int num_iters = 100;
    for (int iter = 0; iter < num_iters; ++iter) {
        auto raw_data = DataGen(schema, N, iter);
        auto new_age_col = raw_data.get_col<int>(1);
        age_col.insert(age_col.end(), new_age_col.begin(), new_age_col.end());
        seg->PreInsert(N);
        seg->Insert(iter * N, N, raw_data.row_ids_.data(), raw_data.timestamps_.data(), raw_data.raw_);
    }

    auto seg_promote = dynamic_cast<SegmentGrowingImpl*>(seg.get());
    ExecExprVisitor visitor(*seg_promote, seg_promote->get_row_count(), MAX_TIMESTAMP);
    for (auto [clause, ref_func] : testcases) {
        Json dsl;
        dsl["bool"] = clause;
        // std::cout << dsl.dump(2);
        auto plan = CreatePlan(*schema, dsl.dump());
        auto final = visitor.call_child(*plan->plan_node_->predicate_.value());
        EXPECT_EQ(final.size(), N * num_iters);

        for (int i = 0; i < N * num_iters; ++i) {
            bool ans = final[i];
            auto val = age_col[i];
            auto ref = ref_func(val);
            ASSERT_EQ(ans, ref) << clause << "@" << i << "!!" << val;
        }
    }
}

TEST(Expr, TestCompare) {
    using namespace milvus::query;
    using namespace milvus::segcore;
    std::vector<std::tuple<std::string, std::function<bool(int, int64_t)>>> testcases = {
        {R"("LT")", [](int a, int64_t b) { return a < b; }},  {R"("LE")", [](int a, int64_t b) { return a <= b; }},
        {R"("GT")", [](int a, int64_t b) { return a > b; }},  {R"("GE")", [](int a, int64_t b) { return a >= b; }},
        {R"("EQ")", [](int a, int64_t b) { return a == b; }}, {R"("NE")", [](int a, int64_t b) { return a != b; }},
    };

    std::string dsl_string_tpl = R"(
{
    "bool": {
        "must": [
            {
                "compare": {
                    %1%: [
                        "age1",
                        "age2"
                    ]
                }
            },
            {
                "vector": {
                    "fakevec": {
                        "metric_type": "L2",
                        "params": {
                            "nprobe": 10
                        },
                        "query": "$0",
                        "topk": 10,
                        "round_decimal": 3
                    }
                }
            }
        ]
    }
})";
    auto schema = std::make_shared<Schema>();
    schema->AddDebugField("fakevec", DataType::VECTOR_FLOAT, 16, MetricType::METRIC_L2);
    schema->AddDebugField("age1", DataType::INT32);
    schema->AddDebugField("age2", DataType::INT64);

    auto seg = CreateGrowingSegment(schema);
    int N = 1000;
    std::vector<int> age1_col;
    std::vector<int64_t> age2_col;
    int num_iters = 100;
    for (int iter = 0; iter < num_iters; ++iter) {
        auto raw_data = DataGen(schema, N, iter);
        auto new_age1_col = raw_data.get_col<int>(1);
        auto new_age2_col = raw_data.get_col<int64_t>(2);
        age1_col.insert(age1_col.end(), new_age1_col.begin(), new_age1_col.end());
        age2_col.insert(age2_col.end(), new_age2_col.begin(), new_age2_col.end());
        seg->PreInsert(N);
        seg->Insert(iter * N, N, raw_data.row_ids_.data(), raw_data.timestamps_.data(), raw_data.raw_);
    }

    auto seg_promote = dynamic_cast<SegmentGrowingImpl*>(seg.get());
    ExecExprVisitor visitor(*seg_promote, seg_promote->get_row_count(), MAX_TIMESTAMP);
    for (auto [clause, ref_func] : testcases) {
        auto dsl_string = boost::str(boost::format(dsl_string_tpl) % clause);
        auto plan = CreatePlan(*schema, dsl_string);
        // std::cout << ShowPlanNodeVisitor().call_child(*plan->plan_node_) << std::endl;
        auto final = visitor.call_child(*plan->plan_node_->predicate_.value());
        EXPECT_EQ(final.size(), N * num_iters);

        for (int i = 0; i < N * num_iters; ++i) {
            auto ans = final[i];

            auto val1 = age1_col[i];
            auto val2 = age2_col[i];
            auto ref = ref_func(val1, val2);
            ASSERT_EQ(ans, ref) << clause << "@" << i << "!!" << boost::format("[%1%, %2%]") % val1 % val2;
        }
    }
}

max_stars_repo_name: elfisworking/milvus

max_issues_repo_name: ruixiaodantiandao/milvus

max_forks_repo_name: ruixiaodantiandao/milvus


text:
//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_POSMIN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_POSMIN_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, posmin_, boost::dispatch::elementwise_<posmin_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, posmin_);
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::posmin_,posmin);

} }

#endif


text:
#ifndef __wrapper_MSSMNoFV_onshell_soft_parameters_def_gm2calc_1_2_0_hpp__
#define __wrapper_MSSMNoFV_onshell_soft_parameters_def_gm2calc_1_2_0_hpp__

#include "wrapper_MSSMNoFV_onshell_susy_parameters_decl.hpp"
#include <Eigen/Core>
#include <ostream>

#include "identification.hpp"

namespace CAT_3(BACKENDNAME,_,SAFE_VERSION)
{

   namespace gm2calc
   {

      // Member functions:
      inline void MSSMNoFV_onshell_soft_parameters::print(::std::basic_ostream<char, std::char_traits<char> >& arg_1) const
      {
         get_BEptr()->print(arg_1);
      }

      inline void MSSMNoFV_onshell_soft_parameters::clear()
      {
         get_BEptr()->clear();
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_TYd(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYd_)
      {
         get_BEptr()->set_TYd(TYd_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_TYd(int i, int k, double value)
      {
         get_BEptr()->set_TYd(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_TYe(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYe_)
      {
         get_BEptr()->set_TYe(TYe_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_TYe(int i, int k, double value)
      {
         get_BEptr()->set_TYe(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_TYu(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYu_)
      {
         get_BEptr()->set_TYu(TYu_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_TYu(int i, int k, double value)
      {
         get_BEptr()->set_TYu(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_BMu(double BMu_)
      {
         get_BEptr()->set_BMu(BMu_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_mq2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mq2_)
      {
         get_BEptr()->set_mq2(mq2_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_mq2(int i, int k, double value)
      {
         get_BEptr()->set_mq2(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_ml2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& ml2_)
      {
         get_BEptr()->set_ml2(ml2_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_ml2(int i, int k, double value)
      {
         get_BEptr()->set_ml2(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_mHd2(double mHd2_)
      {
         get_BEptr()->set_mHd2(mHd2_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_mHu2(double mHu2_)
      {
         get_BEptr()->set_mHu2(mHu2_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_md2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& md2_)
      {
         get_BEptr()->set_md2(md2_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_md2(int i, int k, double value)
      {
         get_BEptr()->set_md2(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_mu2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mu2_)
      {
         get_BEptr()->set_mu2(mu2_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_mu2(int i, int k, double value)
      {
         get_BEptr()->set_mu2(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_me2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& me2_)
      {
         get_BEptr()->set_me2(me2_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_me2(int i, int k, double value)
      {
         get_BEptr()->set_me2(i, k, value);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_MassB(double MassB_)
      {
         get_BEptr()->set_MassB(MassB_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_MassWB(double MassWB_)
      {
         get_BEptr()->set_MassWB(MassWB_);
      }

      inline void MSSMNoFV_onshell_soft_parameters::set_MassG(double MassG_)
      {
         get_BEptr()->set_MassG(MassG_);
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_TYd() const
      {
         return get_BEptr()->get_TYd();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_TYd(int i, int k) const
      {
         return get_BEptr()->get_TYd(i, k);
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_TYe() const
      {
         return get_BEptr()->get_TYe();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_TYe(int i, int k) const
      {
         return get_BEptr()->get_TYe(i, k);
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_TYu() const
      {
         return get_BEptr()->get_TYu();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_TYu(int i, int k) const
      {
         return get_BEptr()->get_TYu(i, k);
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_BMu() const
      {
         return get_BEptr()->get_BMu();
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_mq2() const
      {
         return get_BEptr()->get_mq2();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_mq2(int i, int k) const
      {
         return get_BEptr()->get_mq2(i, k);
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_ml2() const
      {
         return get_BEptr()->get_ml2();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_ml2(int i, int k) const
      {
         return get_BEptr()->get_ml2(i, k);
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_mHd2() const
      {
         return get_BEptr()->get_mHd2();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_mHu2() const
      {
         return get_BEptr()->get_mHu2();
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_md2() const
      {
         return get_BEptr()->get_md2();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_md2(int i, int k) const
      {
         return get_BEptr()->get_md2(i, k);
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_mu2() const
      {
         return get_BEptr()->get_mu2();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_mu2(int i, int k) const
      {
         return get_BEptr()->get_mu2(i, k);
      }

      inline const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& MSSMNoFV_onshell_soft_parameters::get_me2() const
      {
         return get_BEptr()->get_me2();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_me2(int i, int k) const
      {
         return get_BEptr()->get_me2(i, k);
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_MassB() const
      {
         return get_BEptr()->get_MassB();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_MassWB() const
      {
         return get_BEptr()->get_MassWB();
      }

      inline double MSSMNoFV_onshell_soft_parameters::get_MassG() const
      {
         return get_BEptr()->get_MassG();
      }

      // Wrappers for original constructors:
      inline gm2calc::MSSMNoFV_onshell_soft_parameters::MSSMNoFV_onshell_soft_parameters() :
         MSSMNoFV_onshell_susy_parameters(__factory0())
      {
         get_BEptr()->set_wptr(this);
         get_BEptr()->set_delete_wrapper(false);
      }

      inline gm2calc::MSSMNoFV_onshell_soft_parameters::MSSMNoFV_onshell_soft_parameters(const gm2calc::MSSMNoFV_onshell_susy_parameters& arg_1, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYd_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYe_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYu_, double BMu_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mq2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& ml2_, double mHd2_, double mHu2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& md2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mu2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& me2_, double MassB_, double MassWB_, double MassG_) :
         MSSMNoFV_onshell_susy_parameters(__factory1(arg_1, TYd_, TYe_, TYu_, BMu_, mq2_, ml2_, mHd2_, mHu2_, md2_, mu2_, me2_, MassB_, MassWB_, MassG_))
      {
         get_BEptr()->set_wptr(this);
         get_BEptr()->set_delete_wrapper(false);
      }

      // Special pointer-based constructor:
      inline gm2calc::MSSMNoFV_onshell_soft_parameters::MSSMNoFV_onshell_soft_parameters(gm2calc::Abstract_MSSMNoFV_onshell_soft_parameters* in) :
         MSSMNoFV_onshell_susy_parameters(in)
      {
         get_BEptr()->set_wptr(this);
         get_BEptr()->set_delete_wrapper(false);
      }

      // Copy constructor:
      inline gm2calc::MSSMNoFV_onshell_soft_parameters::MSSMNoFV_onshell_soft_parameters(const MSSMNoFV_onshell_soft_parameters& in) :
         MSSMNoFV_onshell_susy_parameters(in.get_BEptr()->pointer_copy__BOSS())
      {
         get_BEptr()->set_wptr(this);
         get_BEptr()->set_delete_wrapper(false);
      }

      // Assignment operator:
      inline gm2calc::MSSMNoFV_onshell_soft_parameters& MSSMNoFV_onshell_soft_parameters::operator=(const MSSMNoFV_onshell_soft_parameters& in)
      {
         if (this != &in)
         {
            get_BEptr()->pointer_assign__BOSS(in.get_BEptr());
         }
         return *this;
      }

      // Destructor:
      inline gm2calc::MSSMNoFV_onshell_soft_parameters::~MSSMNoFV_onshell_soft_parameters()
      {
         if (get_BEptr() != 0)
         {
            get_BEptr()->set_delete_wrapper(false);
            if (can_delete_BEptr())
            {
               delete BEptr;
               BEptr = 0;
            }
         }
         set_delete_BEptr(false);
      }

      // Returns correctly casted pointer to Abstract class:
      inline gm2calc::Abstract_MSSMNoFV_onshell_soft_parameters* gm2calc::MSSMNoFV_onshell_soft_parameters::get_BEptr() const
      {
         return dynamic_cast<gm2calc::Abstract_MSSMNoFV_onshell_soft_parameters*>(BEptr);
      }
   }

}

#include "gambit/Backends/backend_undefs.hpp"

#endif /* __wrapper_MSSMNoFV_onshell_soft_parameters_def_gm2calc_1_2_0_hpp__ */