text:
/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <algorithm>
#include <iterator>
#include <boost/cstdint.hpp>
#include <sprig/binary.hpp>
#include <sprig/utility/iterator.hpp>
#include <sprig/uck/traits.hpp>
#include <sprig/uck/types.hpp>
#include <sprig/uck/std/std_types.hpp>
#include <sprig/uck/fun/fun_types.hpp>
#include <sprig/uck/fun/fun_manager.hpp>

namespace sprig {
	namespace uck {
		//
		// fun_arithmetic_assign
		//
		template<typename Traits = traits<> >
		struct fun_arithmetic_assign {
		public:
			typedef Traits traits_type;
			typedef types<traits_type> types_type;
			typedef std_types<traits_type> std_types_type;
			typedef fun_types<traits_type> fun_types_type;
		public:
			static typename fun_types_type::name_type const& name() {
				static typename fun_types_type::name_type const v("arithmetic_assign");
				return v;
			}
		public:
			//
			// plus_assign_int32
			//
			struct plus_assign_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("+=i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v += get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// plus_assign_uint32
			//
			struct plus_assign_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("+=u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v += get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// minus_assign_int32
			//
			struct minus_assign_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("-=i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v -= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// minus_assign_uint32
			//
			struct minus_assign_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("-=u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v -= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// multiplies_assign_int32
			//
			struct multiplies_assign_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("*=i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v *= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// multiplies_assign_uint32
			//
			struct multiplies_assign_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("*=u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v *= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// devides_assign_int32
			//
			struct devides_assign_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("/=i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v /= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// devides_assign_uint32
			//
			struct devides_assign_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("/=u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v /= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
			//
			// modulus_assign_int32
			//
			struct modulus_assign_int32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("%=i4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::int32_t v = get_from_big_endian<boost::int32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v %= get_from_big_endian<boost::int32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::int32_t>(v, res_first, res_last);
				}
			};
			//
			// modulus_assign_uint32
			//
			struct modulus_assign_uint32 {
			public:
				static typename fun_types_type::name_type const& name() {
					static typename fun_types_type::name_type const v("%=u4");
					return v;
				}
			public:
				void operator()(
					typename fun_types_type::fun_argument_type arg_first,
					typename fun_types_type::fun_argument_type arg_last,
					typename fun_types_type::fun_result_type res_first,
					typename fun_types_type::fun_result_type res_last,
					typename std_types_type::work_argument_type work_first,
					typename std_types_type::work_argument_type work_last
					) const
				{
					if (std::distance(arg_first, arg_last) < 1) {
						return;
					}
					boost::uint32_t v = get_from_big_endian<boost::uint32_t>(res_first, res_last);
					for (typename fun_types_type::fun_argument_type i = arg_first, last = arg_last; i != last; ++i) {
						v %= get_from_big_endian<boost::uint32_t>(i->begin(), i->end());
					}
					std::fill(res_first, res_last, 0);
					set_to_big_endian<boost::uint32_t>(v, res_first, res_last);
				}
			};
		public:
			static typename fun_types_type::package_type const& package() {
				typedef typename fun_types_type::package_type result_type;
				typedef typename result_type::value_type value_type;
				static result_type v;
				struct initializer {
					int operator()() const {
						v.insert(value_type(plus_assign_int32::name(), plus_assign_int32()));
						v.insert(value_type(plus_assign_uint32::name(), plus_assign_uint32()));
						v.insert(value_type(minus_assign_int32::name(), minus_assign_int32()));
						v.insert(value_type(minus_assign_uint32::name(), minus_assign_uint32()));
						v.insert(value_type(multiplies_assign_int32::name(), multiplies_assign_int32()));
						v.insert(value_type(multiplies_assign_uint32::name(), multiplies_assign_uint32()));
						v.insert(value_type(devides_assign_int32::name(), devides_assign_int32()));
						v.insert(value_type(devides_assign_uint32::name(), devides_assign_uint32()));
						v.insert(value_type(modulus_assign_int32::name(), modulus_assign_int32()));
						v.insert(value_type(modulus_assign_uint32::name(), modulus_assign_uint32()));
						return 0;
					}
				};
				static int const init = initializer()();
				return v;
			}
			static bool attach() {
				return fun_manager<traits_type>::get_mutable_instance().attach(name(), package().begin(), package().end());
			}
			static bool detach() {
				return fun_manager<traits_type>::get_mutable_instance().detach(name());
			}
		};
	}	// namespace uck
}	// namespace sprig

#endif	// #ifndef SPRIG_UCK_FUN_FUN_ARITHMETIC_ASSIGN_HPP

ext: hpp

max_stars_repo_name: bolero-MURAKAMI/Sprig

max_issues_repo_name: bolero-MURAKAMI/Sprig

max_forks_repo_name: bolero-MURAKAMI/Sprig


text:
#include "refill/distributions/distribution_base.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>

namespace refill {

class DistributionBaseClass : public DistributionBase<DistributionBaseClass> {
  Eigen::VectorXd mean() const override { return Eigen::VectorXd::Zero(1); }

  Eigen::MatrixXd cov() const override { return Eigen::MatrixXd::Ones(1, 1); }

  Eigen::VectorXd drawSample() override { return Eigen::VectorXd::Zero(1); }

  double evaluatePdf(const Eigen::VectorXd& x) const override { return 0.0; }
};

TEST(DistributionBaseTest, VectorizedPdfEvaluationTest) {
  DistributionBaseClass distribution;

  EXPECT_EQ(Eigen::Vector2d::Zero(2),
            distribution.evaluatePdfVectorized(Eigen::MatrixXd::Ones(2, 2)));
}

TEST(DistributionBaseTest, RngAccessorTest) {
  DistributionBaseClass distribution;

  std::mt19937 rng;

  distribution.setRng(rng);

  std::mt19937 dist_rng = distribution.getRng();

  EXPECT_EQ(rng, dist_rng);
}

}  // namespace refill

ext: cc

max_stars_repo_name: jwidauer/refill

max_issues_repo_name: jwidauer/refill

max_forks_repo_name: jwidauer/refill


text:
#include "multimodal_act.hpp"
#include "paddle/include/paddle_inference_api.h"
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <chrono>
#include <cmath>
#include <ctime>
#include <deque>
#include <fstream>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <iostream>
#include <numeric>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/video.hpp>
#include <opencv2/videoio.hpp>
#include <random>
#include <set>
#include <sstream>
#include <stdexcept>

#ifdef SERVER_MODE
#include "eval_server.grpc.pb.h"
#include <grpcpp/ext/proto_server_reflection_plugin.h>
#include <grpcpp/grpcpp.h>
#include <grpcpp/health_check_service_interface.h>
#include <thread>
#endif

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    RowMajorMatrixXf;
typedef Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    RowMajorMatrixXi;
typedef Eigen::Matrix<int64_t, Eigen::Dynamic, 1> VectorXi64;

const double eps = 0.000001;
const int NUM_ACT = 317;
const int IMG_RESIZE_0 = 416;
const int IMG_RESIZE = 224;
const int VIEW_H = 360; // 720 / 2
const int VIEW_W = 640; // 1280 / 2
const int OB_WINDOW_LEN = 8;
const int SAFE_ACTS[] = {1, 3, 4, 5, 6, 7, 8, 10};

struct EvalEntry {
  int label;
  int waeID;
  std::string dir;
  std::vector<std::string> frameIDs;
  std::vector<Eigen::Vector4f> bboxLst;
};

std::deque<cv::Mat> obWindow;
std::deque<Eigen::VectorXf> processedImgWindow;

DEFINE_string(dirname, "./baseline_r2plus1d",
              "Directory of the inference model and params.");
DEFINE_double(tau, 1.0, "Softmax temperature hyperparameter.");
DEFINE_int32(topK, 50, "Number of top-k multimodal actions.");
DEFINE_bool(gpu, false, "Whether to use GPU config.");
DEFINE_string(logdir, "./log",
              "Directory of the log, include observations, response JSON.");
DEFINE_string(dataTxt, "/mnt/xueyang/Code/xiaodu-hi/data/final_eval.txt",
              "Path to evaluation dataset txt file.");
DEFINE_string(outputType, "wae",
              "Output type of R(2+1)D model, options: wae, scenario.");

#ifdef SERVER_MODE
DEFINE_int32(port, 8888, "Port of gRPC server to bind.");
#endif

template <typename T> void PrintVectorX(std::string name, T v, size_t n) {
  std::cout << "[" << name << "] ";
  std::cout << "size: " << v.size() << ", first " << n << ": ";
  for (size_t i = 0; i < n; i++)
    std::cout << v(i) << " ";
  std::cout << std::endl;
}

template <typename T>
std::vector<size_t> ArgSort(const std::vector<T> &v, bool ascending = false) {
  std::vector<size_t> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);
  if (ascending)
    std::stable_sort(idx.begin(), idx.end(),
                     [&v](size_t i1, size_t i2) { return v[i1] < v[i2]; });
  else
    std::stable_sort(idx.begin(), idx.end(),
                     [&v](size_t i1, size_t i2) { return v[i1] > v[i2]; });
  return idx;
}

std::vector<size_t> ArgSort(const Eigen::VectorXf &v, bool ascending = false) {
  std::vector<float> vec;
  vec.resize(v.size());
  for (int i = 0; i < v.size(); i++)
    vec[i] = v(i);

  return ArgSort<float>(vec, ascending);
}

int GetCurrentHour() {
  auto now = std::chrono::system_clock::now();
  std::time_t t_now = std::chrono::system_clock::to_time_t(now);
  tm *date = localtime(&t_now);
  return static_cast<int>(date->tm_hour);
}

void PreprocessImage(cv::Mat &image, Eigen::VectorXf &flattenOutput,
                     std::string outputImgFile = "") {
  double aspectRatio = std::min(IMG_RESIZE_0 * 1.0 / image.rows,
                                IMG_RESIZE_0 * 1.0 / image.cols);
  int newH = static_cast<int>(std::floor(image.rows * aspectRatio));
  int newW = static_cast<int>(std::floor(image.cols * aspectRatio));
  cv::resize(image, image, cv::Size(newW, newH));

  cv::Mat boxedImg(IMG_RESIZE_0, IMG_RESIZE_0, CV_8UC3,
                   cv::Scalar(128, 128, 128));

  int yOffset = static_cast<int>(std::floor(IMG_RESIZE_0 - newH) / 2.0);
  int xOffset = static_cast<int>(std::floor(IMG_RESIZE_0 - newW) / 2.0);
  image.copyTo(boxedImg(cv::Rect(xOffset, yOffset, newW, newH)));
  cv::resize(boxedImg, boxedImg, cv::Size(IMG_RESIZE, IMG_RESIZE));

  if (outputImgFile != "")
    cv::imwrite(outputImgFile, boxedImg);

  cv::Mat rgbImg;
  cv::cvtColor(boxedImg, rgbImg, cv::COLOR_BGRA2RGB);

  cv::Mat rgbMat[3];
  cv::split(rgbImg, rgbMat);

  Eigen::MatrixXf rMat, gMat, bMat;
  cv::cv2eigen(rgbMat[0], rMat);
  cv::cv2eigen(rgbMat[1], gMat);
  cv::cv2eigen(rgbMat[2], bMat);

  rMat /= 255.0;
  gMat /= 255.0;
  bMat /= 255.0;

  float rMean = 0.43216, gMean = 0.394666, bMean = 0.37645;
  float rStd = 0.22803, gStd = 0.22145, bStd = 0.216989;
  rMat = rMat - rMean * Eigen::MatrixXf::Ones(rMat.rows(), rMat.cols());
  gMat = gMat - gMean * Eigen::MatrixXf::Ones(gMat.rows(), gMat.cols());
  bMat = bMat - bMean * Eigen::MatrixXf::Ones(bMat.rows(), bMat.cols());
  rMat /= rStd;
  gMat /= gStd;
  bMat /= bStd;

  // Flatten RGB mode data with shape [C, H, W]
  RowMajorMatrixXf t_rMat(rMat), t_gMat(gMat), t_bMat(bMat);
  flattenOutput.resize(t_rMat.size() + t_gMat.size() + t_bMat.size());
  flattenOutput << Eigen::Map<Eigen::VectorXf>(t_rMat.data(), t_rMat.size()),
      Eigen::Map<Eigen::VectorXf>(t_gMat.data(), t_gMat.size()),
      Eigen::Map<Eigen::VectorXf>(t_bMat.data(), t_bMat.size());
}

void PrepareMultimodalActions(std::string filename,
                              std::vector<MultimodalAction> &multimodalActs) {
  std::ifstream infile(filename);
  std::string talk, exp, act;
  while (true) {
    if (!std::getline(infile, talk))
      break;
    std::getline(infile, exp);
    std::getline(infile, act);

    MultimodalAction ma(talk, exp, act);
    multimodalActs.push_back(ma);
  }
}

namespace paddle {
using paddle::AnalysisConfig;

using Time = decltype(std::chrono::high_resolution_clock::now());
Time time() { return std::chrono::high_resolution_clock::now(); };
double TimeDiff(Time t1, Time t2) {
  typedef std::chrono::microseconds ms;
  auto diff = t2 - t1;
  ms counter = std::chrono::duration_cast<ms>(diff);
  return counter.count() / 1000.0;
}

void PrepareTRTConfig(AnalysisConfig *config, std::string name) {
  std::string paramsFile = FLAGS_dirname + "/" + name + "_params";
  std::string modelFile = FLAGS_dirname + "/" + name + "_model";
  bool modelOnly = !boost::filesystem::exists(paramsFile);

  if (modelOnly)
    config->SetModel(modelFile);
  else
    config->SetModel(modelFile, paramsFile);

  if (FLAGS_gpu) {
    // Init GPU memory: 1000MB, GPU id: 0
    config->EnableUseGpu(1000, 0);
  } else {
    config->DisableGpu();
    config->SetCpuMathLibraryNumThreads(8);
  }
  config->SwitchUseFeedFetchOps(false);
  config->SwitchSpecifyInputNames(true);
  config->SwitchIrOptim(true);
}

void RunCtrl(PaddlePredictor *predictor,
             const std::vector<Eigen::VectorXf> &imgArray,
             Eigen::VectorXf &flattenActPred,
             Eigen::VectorXf &flattenActTopKSample) {
  auto time1 = time();

  if (imgArray.size() != OB_WINDOW_LEN)
    throw std::runtime_error("Image array should have size " +
                             std::to_string(OB_WINDOW_LEN));

  flattenActPred.resize(NUM_ACT);
  flattenActTopKSample.resize(1);

  auto imgInput = predictor->GetInputTensor("x2paddle_0");
  // For [1, 3, 8, 224, 224] input
  // imgInput->Reshape({1, 3, OB_WINDOW_LEN, IMG_RESIZE, IMG_RESIZE});
  // int lastDim = IMG_RESIZE * IMG_RESIZE;
  // RowMajorMatrixXf imgTensor(3 * OB_WINDOW_LEN, lastDim);
  // for (int i = 0; i < 3; i++) {
  //   for (int j = 0; j < OB_WINDOW_LEN; j++) {
  //     int k = i * OB_WINDOW_LEN + j;
  //     imgTensor.row(k) = imgArray[j].segment(i * lastDim, lastDim);
  //   }
  // }

  // For [1, 8, 3, 224, 224] input
  imgInput->Reshape({1, OB_WINDOW_LEN, 3, IMG_RESIZE, IMG_RESIZE});
  RowMajorMatrixXf imgTensor(OB_WINDOW_LEN, imgArray[0].size());
  for (int i = 0; i < OB_WINDOW_LEN; i++)
    imgTensor.row(i) = imgArray[i];

  imgInput->copy_from_cpu(imgTensor.data());

  auto softmaxTemp = predictor->GetInputTensor("softmax_temp");
  softmaxTemp->Reshape({1});
  Eigen::VectorXf tau;
  tau.resize(1);
  tau(0) = FLAGS_tau;
  softmaxTemp->copy_from_cpu(tau.data());

  auto topKInput = predictor->GetInputTensor("top_k");
  topKInput->Reshape({1});
  VectorXi64 topK;
  topK.resize(1);
  topK(0) = FLAGS_topK;
  topKInput->copy_from_cpu(topK.data());

  CHECK(predictor->ZeroCopyRun());

  auto outputNames = predictor->GetOutputNames();
  auto actPred = predictor->GetOutputTensor(outputNames[0]);
  auto actTopKSample = predictor->GetOutputTensor(outputNames[1]);

  actPred->copy_to_cpu(flattenActPred.data());
  actTopKSample->copy_to_cpu(flattenActTopKSample.data());

  auto time2 = time();
  LOG(INFO) << "[RunCtrl] cost: " << TimeDiff(time1, time2) << "ms"
            << std::endl;
}

bool ConvertPredToJsons(int reqID, bool useSkill,
                        const Eigen::VectorXf &actPred,
                        const Eigen::VectorXf &actTopKSample,
                        const std::vector<MultimodalAction> &multimodalActs,
                        std::string &resJson, float &resActScore) {
  auto time1 = time();
  size_t nullActAt = ArgSort(actPred)[0];
  // std::cout << "=========== " << nullActAt << std::endl;
  if (nullActAt == 0) {
    resJson = "{}";
    return false;
  }

  std::default_random_engine rnd;
  rnd.seed(std::chrono::system_clock::now().time_since_epoch().count());
  std::uniform_real_distribution<double> uniform(0, 1);

  int safeActNum = sizeof(SAFE_ACTS) / sizeof(SAFE_ACTS[0]);
  int sampleID = static_cast<int>(actTopKSample(actTopKSample.size() - 1));
  std::string talk = multimodalActs[sampleID].talk;

  if (talk.find("C") != talk.npos) {
    // ignore the salutation.
    sampleID = SAFE_ACTS[static_cast<int>(uniform(rnd) * safeActNum)];
  }

  int hour = GetCurrentHour();
  MultimodalAction ma = multimodalActs[sampleID];
  resJson = ma.to_json(hour, reqID, useSkill, "", "你");
  resActScore = actPred(sampleID);

  auto time2 = time();
  LOG(INFO) << "[ConvertPredToJsons] cost: " << TimeDiff(time1, time2) << "ms"
            << std::endl;
  return true;
}

} // namespace paddle

void RunEval(paddle::PaddlePredictor *ctrlPredictor,
             const std::vector<MultimodalAction> &multimodalActs, EvalEntry ee,
             bool &hasAct, std::string &resJson, float &annoActScore) {
  obWindow.clear();
  processedImgWindow.clear();

  int offset = ee.frameIDs.size() - OB_WINDOW_LEN;
  if (offset < 0)
    throw "Evaluation entry has frames less than " +
        std::to_string(OB_WINDOW_LEN);

  for (int i = offset; i < ee.frameIDs.size(); i++) {
    std::string imgFile = ee.dir + "/" + ee.frameIDs[i] + ".jpg";
    cv::Mat img = cv::imread(imgFile);

    Eigen::VectorXf flattenImg;
    PreprocessImage(img, flattenImg);

    obWindow.push_back(img);
    processedImgWindow.push_back(flattenImg);
  }

  Eigen::VectorXf flattenActPred, flattenActTopKSample;
  paddle::RunCtrl(ctrlPredictor,
                  {processedImgWindow.begin(), processedImgWindow.end()},
                  flattenActPred, flattenActTopKSample);

  float resActScore;
  hasAct =
      paddle::ConvertPredToJsons(0, false, flattenActPred, flattenActTopKSample,
                                 multimodalActs, resJson, resActScore);
  annoActScore = flattenActPred(ee.waeID);
}

#ifdef SERVER_MODE
using evalserver::EvalRequest;
using evalserver::EvalResponse;
using evalserver::EvalServer;
using grpc::Server;
using grpc::ServerBuilder;
using grpc::ServerContext;
using grpc::Status;

void DecodeFrames(const EvalRequest *request, std::vector<cv::Mat> &frames) {
  int nframe = request->nframe();
  std::string bytesStr = request->frames();
  cv::Mat merge(VIEW_H * nframe, VIEW_W, CV_8UC3,
                const_cast<char *>(bytesStr.c_str()));

  for (int i = 0; i < nframe; i++) {
    cv::Rect rect(0, VIEW_H * i, VIEW_W, VIEW_H);
    frames.push_back(merge(rect).clone());
  }
}

class EvalServiceImpl final : public EvalServer::Service {
public:
  EvalServiceImpl(paddle::PaddlePredictor *ctrlPredictor)
      : ctrlPredictor(ctrlPredictor) {
    std::string actPath = FLAGS_dirname + "/" + "multimodal_actions.txt";
    PrepareMultimodalActions(actPath, multimodalActs);
  }

  Status infer(ServerContext *context, const EvalRequest *request,
               EvalResponse *reply) override {
    std::vector<cv::Mat> frames;
    DecodeFrames(request, frames);

    obWindow.clear();
    processedImgWindow.clear();

    for (auto img : frames) {
      Eigen::VectorXf flattenImg;
      PreprocessImage(img, flattenImg);

      obWindow.push_back(img);
      processedImgWindow.push_back(flattenImg);
    }

    Eigen::VectorXf flattenActPred, flattenActTopKSample;
    paddle::RunCtrl(ctrlPredictor,
                    {processedImgWindow.begin(), processedImgWindow.end()},
                    flattenActPred, flattenActTopKSample);

    float resActScore;
    std::string resJson;
    bool hasAct = paddle::ConvertPredToJsons(
        0, false, flattenActPred, flattenActTopKSample, multimodalActs, resJson,
        resActScore);
    int nullActAt = ArgSort(flattenActPred)[0];

    reply->set_response(resJson);
    reply->set_response_score(resActScore);
    reply->set_trigger_pred(0.0);
    reply->set_nullact_score(flattenActPred(0));
    reply->set_nullact_id(nullActAt);
    return Status::OK;
  }

private:
  paddle::PaddlePredictor *ctrlPredictor;
  std::vector<MultimodalAction> multimodalActs;
};

void RunServer(paddle::PaddlePredictor *ctrlPredictor) {
  std::string server_address("0.0.0.0:" + std::to_string(FLAGS_port));
  grpc::EnableDefaultHealthCheckService(true);
  grpc::reflection::InitProtoReflectionServerBuilderPlugin();

  EvalServiceImpl service(ctrlPredictor);
  ServerBuilder builder;
  builder.AddListeningPort(server_address, grpc::InsecureServerCredentials());
  builder.RegisterService(&service);
  builder.SetMaxReceiveMessageSize(8 * 1024 * 1024); // larger than 6912007
  std::unique_ptr<Server> server(builder.BuildAndStart());
  std::cout << "Server listening on " << server_address << std::endl;
  server->Wait();
}

#endif // end of server mode

using namespace std;

int main(int argc, char *argv[]) {
  gflags::SetUsageMessage("Usage : ./eval_r2plus1d ");
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  LOG(INFO) << "Directory of the inference model and params: " << FLAGS_dirname
            << endl;

  std::string actPath = FLAGS_dirname + "/" + "multimodal_actions.txt";
  std::vector<MultimodalAction> multimodalActs;
  PrepareMultimodalActions(actPath, multimodalActs);

  paddle::AnalysisConfig ctrlCfg;
  paddle::PrepareTRTConfig(&ctrlCfg, "ctrl");
  auto ctrlPredictor = paddle::CreatePaddlePredictor(ctrlCfg);
  LOG(INFO) << "Created controller model predictor" << endl;

#ifndef SERVER_MODE
  // ==================================================
  // start of evaluation using -dataTxt
  // ==================================================

  int eeID = 0, nullActTP = 0, nullActFP = 0, nullActFN = 0;
  float actNLL = 0.0;

  if (!boost::filesystem::exists(FLAGS_logdir))
    boost::filesystem::create_directories(FLAGS_logdir);

  ofstream nullActFPLog(FLAGS_logdir + "/null_act_fp.txt");
  ofstream nullActFNLog(FLAGS_logdir + "/null_act_fn.txt");
  ofstream metricLog(FLAGS_logdir + "/metric.txt");

  ifstream infile(FLAGS_dataTxt);
  string line;
  while (true) {
    if (!getline(infile, line))
      break;

    istringstream ss(line);
    EvalEntry ee;
    ss >> ee.label;
    ss >> ee.dir;

    string tmp, frameID;
    ss >> tmp;
    istringstream tt(tmp);
    while (getline(tt, frameID, ',')) {
      ee.frameIDs.push_back(frameID);
    }

    if (ee.label == 1) {
      int scenarioID; // only useful for baselines
      ss >> ee.waeID;
      ss >> scenarioID;

      while (ss) {
        ss >> tmp;
        istringstream bb(tmp);

        string str;
        Eigen::Vector4f bbox;
        int i = 0;
        while (getline(bb, str, ',')) {
          bbox(i) = stof(str);
          i++;
        }

        if (ss)
          ee.bboxLst.push_back(bbox);
      }

    } else if (ee.label == 0) {
      ee.waeID = 0;
    }

    bool hasAct;
    string resJson;
    float annoActScore;
    RunEval(static_cast<paddle::PaddlePredictor *>(ctrlPredictor.get()),
            multimodalActs, ee, hasAct, resJson, annoActScore);

    if (hasAct && ee.label == 1)
      nullActTP++;
    else if (hasAct && ee.label == 0) {
      nullActFP++;
      nullActFPLog << line << endl;
    } else if (!hasAct && ee.label == 1) {
      nullActFN++;
      nullActFNLog << line << endl;
    }

    actNLL += -log(annoActScore);

    eeID++;
    cout << "=========== " << eeID << endl;

    // cout << "===== Eval entry =====" << endl;
    // cout << ee.label << " " << ee.waeID << endl;
    // cout << ee.dir << endl;
    // for (auto i : ee.frameIDs)
    //   cout << i << " ";
    // cout << endl;
    // for (auto i : ee.bboxLst)
    //   cout << "[" << i(0) << ", " << i(1) << ", " << i(2) << ", " << i(3) <<
    //   "]"
    //        << endl;
  }

  double nullActPrecision = (nullActTP + eps) / (nullActTP + nullActFP + eps);
  double nullActRecall = (nullActTP + eps) / (nullActTP + nullActFN + eps);
  cout << "==============================" << endl;
  cout << "Null Act" << endl;
  cout << "Precision: " << nullActPrecision << endl;
  cout << "Recall: " << nullActRecall << endl;
  cout << "==============================" << endl;

  double avgActNLL = actNLL / eeID;
  cout << "Act Average NLL: " << avgActNLL << endl;
  cout << "==============================" << endl;

  metricLog << "#NullAct" << endl;
  metricLog << "Precision Recall" << endl;
  metricLog << nullActPrecision << " " << nullActRecall << endl;
  metricLog << "\nactNLL: " << avgActNLL << endl;
  metricLog << "eeID: " << eeID << endl;

  nullActFPLog.close();
  nullActFNLog.close();
  metricLog.close();

#else
  // ==================================================
  // start of eval server
  // ==================================================
  RunServer(static_cast<paddle::PaddlePredictor *>(ctrlPredictor.get()));

#endif
}

ext: cpp

max_stars_repo_name: WorldEditors/PaddleRobotics

max_issues_repo_name: WorldEditors/PaddleRobotics

max_forks_repo_name: WorldEditors/PaddleRobotics


text:
///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::algorithm::make_mean_events.hpp               //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_VECTORIZE_EVENTS_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_VECTORIZE_EVENTS_HPP_ER_2009
#include <boost/iterator/iterator_traits.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{

// Copies the failure and event time of each event in a sequence to an
// output iterator
template<typename ItE,typename ItO>
ItO vectorize_events(
    ItE b,
    ItE e,
    ItO o
);

    // Implementation //

template<typename ItE,typename ItO>
ItO vectorize_events(
    ItE b,
    ItE e,
    ItO o
){
    typedef typename iterator_value<ItE>::type event_;
    typedef typename event_::value_type val_;
    typedef typename event_::failure_type fail_;
    BOOST_MPL_ASSERT((
        is_same<val_,fail_>
    ));

    while(b!=e){
        *o = (b->failure()); ++o;
        *o = (b->time()); ++o;
        ++b;
    }
    return o;
}

}// data
}// survival
}// statistics
}// boost

#endif


text:
#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

#include "LSLOpt/BFGS.hpp"

#include "ModelSystem.hpp"

int main(int argc, char* argv[])
{
  ModelSystem modelSystem;

  std::cerr << std::setprecision(16);

  double angle_eps = 0.1 * M_PI / 180.0;

  unsigned n_steps = static_cast<unsigned>((2 * M_PI) / angle_eps) + 1;

  for (unsigned i = 0; i < n_steps; ++i) {
    double angle = angle_eps * i;
    double x = modelSystem.x0.norm() * std::sin(angle);
    double y = modelSystem.x0.norm() * std::cos(angle);
    Eigen::VectorXd x0 = Eigen::VectorXd::Zero(1);
    x0[0] = angle;

    double val = modelSystem.value(x0);

    std::cerr << angle << ";" << x << ";" << y << ";" << val << std::endl;
  }
  return 0;
}

max_stars_repo_name: flachsenberg/LSLOpt

max_issues_repo_name: flachsenberg/LSLOpt

max_forks_repo_name: flachsenberg/LSLOpt


text:
#include <Settings.h>
#include <string>
#include <utilstrencodings.h>
#include <DataDirectory.h>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/algorithm/string/case_conv.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/program_options/detail/config_file.hpp>
#include <set>

std::string CopyableSettings::GetArg(const std::string& strArg, const std::string& strDefault) const
{
    if (mapArgs_.count(strArg))
        return mapArgs_.find(strArg)->second;
    return strDefault;
}

int64_t CopyableSettings::GetArg(const std::string& strArg, int64_t nDefault) const
{
    if (mapArgs_.count(strArg))
        return atoi64(mapArgs_.find(strArg)->second);
    return nDefault;
}

static bool InterpretBool(const std::string& strValue)
{
    if (strValue.empty())
        return true;
    return (atoi(strValue) != 0);
}

static void InterpretNegativeSetting(std::string& strKey, std::string& strValue)
{
    if (strKey.length()>3 && strKey[0]=='-' && strKey[1]=='n' && strKey[2]=='o') {
        strKey = "-" + strKey.substr(3);
        strValue = InterpretBool(strValue) ? "0" : "1";
    }
}

bool CopyableSettings::GetBoolArg(const std::string& strArg, bool fDefault) const
{
    if (mapArgs_.count(strArg))
        return InterpretBool(mapArgs_.find(strArg)->second);
    return fDefault;
}

bool CopyableSettings::SoftSetArg(const std::string& strArg, const std::string& strValue)
{
    if (mapArgs_.count(strArg))
        return false;
    mapArgs_[strArg] = strValue;
    return true;
}

bool CopyableSettings::SoftSetBoolArg(const std::string& strArg, bool fValue)
{
    if (fValue)
        return SoftSetArg(strArg, std::string("1"));
    else
        return SoftSetArg(strArg, std::string("0"));
}

void CopyableSettings::ForceRemoveArg(const std::string &strArg)
{
    mapArgs_.erase(strArg);
}

bool CopyableSettings::ParameterIsSet (const std::string& key) const
{
    return mapArgs_.count(key) > 0;
}

std::string CopyableSettings::GetParameter(const std::string& key) const
{
    if(ParameterIsSet(key))
    {
        return mapArgs_.find(key)->second;
    }
    else
    {
        return "";
    }
}

const std::vector<std::string>& CopyableSettings::GetMultiParameter(const std::string& key) const
{
    static std::vector<std::string> empty;
    if(ParameterIsSetForMultiArgs(key))
    {
        return mapMultiArgs_.find(key)->second;
    }
    else
    {
        return empty;
    }
}

void CopyableSettings::SetParameter (const std::string& key, const std::string& value)
{
    mapArgs_[key] = value;
}

void CopyableSettings::ClearParameter ()
{
    mapArgs_.clear();
}

bool CopyableSettings::ParameterIsSetForMultiArgs (const std::string& key) const
{
    return mapMultiArgs_.count(key) > 0;
}

void CopyableSettings::ParseParameters(int argc, const char* const argv[])
{
    ClearParameter();
    mapMultiArgs_.clear();

    for (int i = 1; i < argc; i++) {
        std::string str(argv[i]);
        std::string strValue;
        size_t is_index = str.find('=');
        if (is_index != std::string::npos) {
            strValue = str.substr(is_index + 1);
            str = str.substr(0, is_index);
        }
#ifdef WIN32
        boost::to_lower(str);
        if (boost::algorithm::starts_with(str, "/"))
            str = "-" + str.substr(1);
#endif

        if (str[0] != '-')
            break;

        // Interpret --foo as -foo.
        // If both --foo and -foo are set, the last takes effect.
        if (str.length() > 1 && str[1] == '-')
            str = str.substr(1);
        InterpretNegativeSetting(str, strValue);

        SetParameter(str, strValue);
        mapMultiArgs_[str].push_back(strValue);
    }
}

boost::filesystem::path CopyableSettings::GetConfigFile() const
{
    boost::filesystem::path pathConfigFile(GetArg("-conf", "izzy.conf"));
    if (!pathConfigFile.is_complete())
        pathConfigFile = GetDataDir(false) / pathConfigFile;

    return pathConfigFile;
}

void CopyableSettings::ReadConfigFile()
{
    boost::filesystem::ifstream streamConfig(GetConfigFile());
    if (!streamConfig.good()) {
        // Create empty izzy.conf if it does not exist
        FILE* configFile = fopen(GetConfigFile().string().c_str(), "a");
        if (configFile != NULL)
            fclose(configFile);
        return; // Nothing to read, so just return
    }

    std::set<std::string> setOptions;
    setOptions.insert("*");

    for (boost::program_options::detail::config_file_iterator it(streamConfig, setOptions), end; it != end; ++it) {
        // Don't overwrite existing settings so command line settings override izzy.conf
        std::string strKey = std::string("-") + it->string_key;
        std::string strValue = it->value[0];
        InterpretNegativeSetting(strKey, strValue);
        if (mapArgs_.count(strKey) == 0)
            mapArgs_[strKey] = strValue;
        mapMultiArgs_[strKey].push_back(strValue);
    }
    // If datadir is changed in .conf file:
    ClearDatadirCache();
}

unsigned CopyableSettings::MaxNumberOfPoSCombinableInputs() const
{
    constexpr unsigned int MAX_KERNEL_COMBINED_INPUTS = 20;
    return MAX_KERNEL_COMBINED_INPUTS;
}
int CopyableSettings::MaxFutureBlockDrift() const
{
    constexpr int maximumFutureBlockDrift = 180;
    return maximumFutureBlockDrift;
}

unsigned CopyableSettings::NumerOfParameters() const
{
    return mapArgs_.size();
}
unsigned CopyableSettings::NumerOfMultiParameters() const
{
    return mapMultiArgs_.size();
}

max_stars_repo_name: izzy-developer/core

max_issues_repo_name: izzy-developer/core

max_forks_repo_name: izzy-developer/core


text: #include <boost/spirit/home/qi/binary.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
// Customize
// You need to define `OPTIONAL_UTILITY_NO_CXX14_RETURN_TYPE_DEDUCTION` macro if you compile c++11 mode. e.g. `std=c++11`
// You define `USE_BOOST_LAMBDA` if you want to use Booost.Lambda in sample codes.
//#define OPTIONAL_UTILITY_NO_CXX14_RETURN_TYPE_DEDUCTION
//#define USE_BOOST_LAMBDA
#include "optional_utility.hpp"
#include <boost/optional/optional_io.hpp>
#if defined(USE_BOOST_LAMBDA)
#include <boost/lambda/lambda.hpp>
#endif
#include <iostream>
#include <random>
#include <stdexcept>
#include <string>
using boost::optional;

// returns joined strings if both strings has a value. otherwise none;
optional<std::string> full_name(optional<std::string> const& first_name, optional<std::string> const& last_name)
{
    using optional_utility::flat_map;
    using optional_utility::map;
    return last_name | flat_map([&first_name](std::string const& ln) {
        return first_name | map([&ln](std::string const& fn) {
            return ln + " " + fn;
        });
    });
}

optional<int> divide(int dividend, int divisor)
{
    if (divisor == 0) {
        return boost::none;
    }
    return dividend / divisor;
}

int main()
{
    // `map` and `flat_map`
    {
        // basic usage
        auto const first_name = boost::make_optional<std::string>("John");
        auto const last_name = boost::make_optional<std::string>("Smith");
        std::cout << full_name(first_name, last_name) << std::endl; // John Smith
        std::cout << full_name(first_name, boost::none) << std::endl; // --
        std::cout << full_name(boost::none, last_name) << std::endl; // --
        std::cout << full_name(boost::none, boost::none) << std::endl; // --
    }
    {
        // adaptors are lazy evaluation
        using optional_utility::map;
        auto const op = boost::make_optional<int>(42);
        int counter = 0;
        auto const temp = op
            | map([&counter](int i) { ++counter;  return i * 2; })
            | map([&counter](int i) { ++counter;  return i + 1; });
        // it is not evalutated yet
        std::cout << counter << std::endl; // 0
        // it is evaluated
        optional<int> op2 = temp;
        std::cout << counter << std::endl; // 2
        std::cout << op2 << std::endl; // 85
    }
    // there are 2 ways to explicitly convert to boost::optional
    {
        using boost::make_optional;
        using optional_utility::map;
        auto const temp = make_optional(42)
            | map([](int num) { return num * 2; });
        // 1. explicit type declaration
        {
            optional<int> const op = temp;
        }
        // 2. using to_optional
        {
            using optional_utility::to_optional;
            auto const op = temp
                | to_optional;
        }
    }
    // non-member functions
    {
        using optional_utility::value_or;
        auto const op = divide(42, std::random_device{}() % 2);
        std::cout << value_or(op, 0) << std::endl; // 42 or 0
    }
    {
        using optional_utility::value_or_throw;
        optional<int> const op = boost::none;
        try {
            // throw any exception if optional is none
            value_or_throw<std::runtime_error>(op, "none!");
        } catch (std::exception const& e) {
            std::cout << e.what() << std::endl; // none!
        }
    }
    // you can use member function pointers
    {
        using optional_utility::map;
        auto const  op = boost::make_optional<std::string>("hello");
        optional<std::string::size_type> op2 = op
            | map(&std::string::substr, 1, 3)
            | map(&std::string::length);
        std::cout << op2 << std::endl;
    }
#if defined(USE_BOOST_LAMBDA)
    {
        using optional_utility::map;
        using optional_utility::filter;
        using optional_utility::to_optional;
        using boost::lambda::_1;
        auto const op = boost::make_optional<std::string>("42")
            | map(_1 + _1)
            | map([](std::string const& s) { return std::stoi(s); })
            | filter(_1 % 2 == 0)
            | to_optional;
        std::cout << op << std::endl;
    }
#endif
}


text:
/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <set>
#include <string>
#include <vector>

#include <boost/variant.hpp>

namespace pnmc { namespace properties {

/*------------------------------------------------------------------------------------------------*/

// Integer formulae :: Arithmetic operators
struct integer_constant;
struct integer_sum;
struct integer_product;
struct integer_difference;
struct integer_division;

// Integer formulae :: Petri net operators
struct place_bound
{
  std::vector<std::string> places;
};

struct tokens_count
{
  std::vector<std::string> places;
};

using integer_expression = boost::variant< integer_constant
                                         , boost::recursive_wrapper<integer_sum>
                                         , boost::recursive_wrapper<integer_product>
                                         , boost::recursive_wrapper<integer_difference>
                                         , boost::recursive_wrapper<integer_division>
                                         , place_bound
                                         , tokens_count>;

// Integer formulae :: Arithmetic operators
struct integer_constant
{
  int value;
};

struct integer_sum
{
  std::vector<integer_expression> expressions;
};

struct integer_product
{
  std::vector<integer_expression> expressions;
};

struct integer_difference
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_division
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

/*------------------------------------------------------------------------------------------------*/

// Boolean formulae :: Reachability operators
struct invariant;
struct impossibility;
struct possibility;

// Boolean formulae :: Petri net operators
struct deadlock {};
struct is_fireable
{
  std::vector<std::string> transitions;
};

// Boolean formulae :: Boolean operators
struct true_ {};
struct false_ {};
struct negation;
struct conjunction;
struct disjunction;
struct exclusive_disjonction;
struct implication;
struct equivalence;

// Boolean formulae :: Comparison operators
struct integer_eq;
struct integer_ne;
struct integer_lt;
struct integer_le;
struct integer_gt;
struct integer_ge;

using boolean_expression = boost::variant< boost::recursive_wrapper<invariant>
                                         , boost::recursive_wrapper<impossibility>
                                         , boost::recursive_wrapper<possibility>
                                         , deadlock
                                         , is_fireable
                                         , true_
                                         , false_
                                         , boost::recursive_wrapper<negation>
                                         , boost::recursive_wrapper<conjunction>
                                         , boost::recursive_wrapper<disjunction>
                                         , boost::recursive_wrapper<exclusive_disjonction>
                                         , boost::recursive_wrapper<implication>
                                         , boost::recursive_wrapper<equivalence>
                                         , boost::recursive_wrapper<integer_eq>
                                         , boost::recursive_wrapper<integer_ne>
                                         , boost::recursive_wrapper<integer_lt>
                                         , boost::recursive_wrapper<integer_le>
                                         , boost::recursive_wrapper<integer_gt>
                                         , boost::recursive_wrapper<integer_ge>>;

//// Boolean formulae :: Reachability operators
struct invariant
{
  boolean_expression expression;
};

struct impossibility
{
  boolean_expression expression;
};

struct possibility
{
  boolean_expression expression;
};

// Boolean formulae :: Boolean operators
struct negation
{
  boolean_expression expression;
};

struct conjunction
{
  std::vector<boolean_expression> expressions;
};

struct disjunction
{
  std::vector<boolean_expression> expressions;
};

struct exclusive_disjonction
{
  std::vector<boolean_expression> expressions;
};

struct implication
{
  boolean_expression lhs_expression;
  boolean_expression rhs_expression;
};

struct equivalence
{
  boolean_expression lhs_expression;
  boolean_expression rhs_expression;
};

// Boolean formulae :: Comparison operators
struct integer_eq
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_ne
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_lt
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_le
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_gt
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_ge
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

/*------------------------------------------------------------------------------------------------*/

using formula = boost::variant<boolean_expression, integer_expression>;

/*------------------------------------------------------------------------------------------------*/

struct formulae
{
  bool compute_deadlock;
  std::set<std::string> places_bounds;
  std::set<std::string> fireable_transitions;

  struct boolean
  {
    std::string id;
    std::set<std::string> targets;
    boolean_expression expression;
  };

  struct integer
  {
    std::string id;
    std::set<std::string> targets;
    integer_expression expression;
  };

  std::vector<boolean> booleans;
  std::vector<integer> integers;
};

/*------------------------------------------------------------------------------------------------*/

}} // namespace pnmc::properties

ext: hh

max_stars_repo_name: ahamez/pnmc

max_issues_repo_name: ahamez/pnmc

max_forks_repo_name: ahamez/pnmc


text:
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE netlistDB_large_query_test

#include <boost/test/unit_test.hpp>
#include <functional>
#include <iostream>
#include <fstream>
#include <stdint.h>
#include <random>

#include <netlistDB/netlist.h>
#include <netlistDB/statement_if.h>
#include <netlistDB/hw_type/common.h>
#include <netlistDB/query/query_match.h>
#include <netlistDB/operator_defs.h>
#include <netlistDB/query/query_traverse.h>
#include <netlistDB/utils/timer.h>

#include "test_graphs.h"

using namespace netlistDB;
using namespace netlistDB::query;
using namespace netlistDB::hw_type;
using namespace netlistDB::utils;

BOOST_AUTO_TEST_SUITE( netlistDB_testsuite )
auto & t = hw_int32;
std::vector<FunctionCall*> find_ops(Netlist & ctx, FunctionDef & op) {
	// traverse and count the number of the add operators
	QueryTraverse q(ctx.nodes.size());
	std::vector<iNode*> inputs;
	for (auto n : ctx.nets) {
		if (n->direction == Direction::DIR_IN) {
			inputs.push_back(n);
		}
	}

	boost::mutex mx;

	std::vector<FunctionCall*> ops;
	auto cb = [&ops, &op, &mx] (iNode& n,
			const std::function<void(iNode &)> & select) {
		auto _n = dynamic_cast<FunctionCall*>(&n);
		if (_n and &_n->fn == &op) {
			boost::mutex::scoped_lock lock(mx);
			ops.push_back(_n);
		}
		QueryTraverse::dummy_callback(n, select);
	};
	q.traverse(inputs, cb);

	return ops;
}

BOOST_AUTO_TEST_CASE( query_add ) {
	Netlist ctx("test");
	std::mt19937 rand(0);
	build_random_circuit(ctx, 10, 10, 10, 10, rand);

	QueryMatch query_add;

	auto &r = query_add.sig_in("a", t) + query_add.sig_in("b", t);
	r.direction = Direction::DIR_OUT;

	auto qres = query_add.search(ctx);
	BOOST_CHECK_EQUAL(qres.size(), 29);

	// search from different end
	std::reverse(ctx.nets.begin(), ctx.nets.end());
	qres = query_add.search(ctx);
	BOOST_CHECK_EQUAL(qres.size(), 29);

	auto adds = find_ops(ctx, OpAdd);
	BOOST_CHECK_EQUAL(qres.size(), 29);
}

BOOST_AUTO_TEST_CASE( query_mac ) {
	size_t N = 100;

	Netlist ctx("test");
	std::mt19937 rand(0);
	build_random_circuit(ctx, N, N, N, N, rand);

	QueryMatch query_mac;
	auto &r = query_mac.sig_in("a", t)
			+ (query_mac.sig_in("b", t) * query_mac.sig_in("c", t));
	r.direction = Direction::DIR_OUT;

	auto t = new Timer(std::string("query_mac size:") + std::to_string(N));
	auto qres = query_mac.search(ctx);
	delete t;

	BOOST_CHECK_EQUAL(qres.size(), 479);
}

//____________________________________________________________________________//

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: HardwareIR/netlistDB

max_issues_repo_name: PDFxy/netlistDB

max_forks_repo_name: HardwareIR/hardwareIr


text:
// -*- coding: utf-8 -*-
// Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
#include "commonmanipulation.h"

#include <boost/algorithm/string.hpp>

class BaseManipulation : public ModuleBase
{
public:
    BaseManipulation(EnvironmentBasePtr penv) : ModuleBase(penv) {
        __description = ":Interface Author: Rosen Diankov\n\nVery useful routines for manipulation planning and planning in general. The planners use analytical inverse kinematics and search based techniques. Most of the MoveX commands by default execute the plan on the current robot by calling :meth:`.RobotBase.GetController().SetPath`. This can be disabled by adding 'execute 0' to the command line";
        RegisterCommand("Traj",boost::bind(&BaseManipulation::Traj,this,_1,_2),
                        "Execute a trajectory from a file on the local filesystem");
        RegisterCommand("GrabBody",boost::bind(&BaseManipulation::GrabBody,this,_1,_2),
                        "Robot calls ::Grab on a body with its current manipulator");
        RegisterCommand("ReleaseAll",boost::bind(&BaseManipulation::ReleaseAll,this,_1,_2),
                        "Releases all grabbed bodies (RobotBase::ReleaseAllGrabbed).");
        RegisterCommand("MoveHandStraight",boost::bind(&BaseManipulation::MoveHandStraight,this,_1,_2),
                        "Move the active end-effector in a straight line until collision or IK fails. Parameters:\n\n\
- steplength - the increments in workspace in which the robot tests for the next configuration.\n\n\
- minsteps - The minimum number of steps that need to be taken in order for success to declared. If robot doesn't reach this number of steps, it fails.\n\n\
- maxsteps - The maximum number of steps the robot should take.\n\n\
- direction - The workspace direction to move end effector in.\n\n\
Method wraps the WorkspaceTrajectoryTracker planner. For more details on parameters, check out its documentation.");
        RegisterCommand("MoveManipulator",boost::bind(&BaseManipulation::MoveManipulator,this,_1,_2),
                        "Moves arm joints of active manipulator to a given set of joint values");
        RegisterCommand("MoveActiveJoints",boost::bind(&BaseManipulation::MoveActiveJoints,this,_1,_2),
                        "Moves the current active joints to a specified goal destination:\n\n\
- maxiter - The maximum number of iterations on the internal planner.\n\
- maxtries - The maximum number of times to restart the planner.\n\
- steplength - See PlannerParameters::_fStepLength\n\n");
        RegisterCommand("MoveToHandPosition",boost::bind(&BaseManipulation::_MoveToHandPosition,this,_1,_2),
                        "Move the manipulator's end effector to reach a set of 6D poses. Parameters:\n\n\
- ");
        RegisterCommand("MoveUnsyncJoints",boost::bind(&BaseManipulation::MoveUnsyncJoints,this,_1,_2),
                        "Moves the active joints to a position where the inactive (hand) joints can\n"
                        "fully move to their goal. This is necessary because synchronization with arm\n"
                        "and hand isn't guaranteed.\n"
                        "Options: handjoints savetraj planner");
        RegisterCommand("JitterActive",boost::bind(&BaseManipulation::JitterActive,this,_1,_2),
                        "Jitters the active DOF for a collision-free position.");
        RegisterCommand("SetMinimumGoalPaths",boost::bind(&BaseManipulation::SetMinimumGoalPathsCommand,this,_1,_2),
                        "Sets _minimumgoalpaths for all planner parameters.");
        RegisterCommand("SetPostProcessing",boost::bind(&BaseManipulation::SetPostProcessingCommand,this,_1,_2),
                        "Sets post processing parameters.");
        RegisterCommand("SetRobot",boost::bind(&BaseManipulation::SetRobotCommand,this,_1,_2),
                        "Sets the robot.");
        _minimumgoalpaths=1;
    }

    virtual ~BaseManipulation() {
    }

    virtual void Destroy()
    {
        robot.reset();
        ModuleBase::Destroy();
    }

    virtual void Reset()
    {
        ModuleBase::Reset();
    }

    virtual int main(const std::string& args)
    {
        _fMaxVelMult=1;
        _minimumgoalpaths=1;
        string strRobotName;
        stringstream ss(args);
        ss >> strRobotName;
        robot = GetEnv()->GetRobot(strRobotName);

        string cmd;
        while(!ss.eof()) {
            ss >> cmd;
            if( !ss ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);
            if( cmd == "planner" ) {
                ss >> _strRRTPlannerName;
            }
            else if( cmd == "maxvelmult" ) {
                ss >> _fMaxVelMult;
            }
            if( ss.fail() || !ss ) {
                break;
            }
        }

        PlannerBasePtr planner;
        if( _strRRTPlannerName.size() > 0 ) {
            planner = RaveCreatePlanner(GetEnv(),_strRRTPlannerName);
        }
        if( !planner ) {
            _strRRTPlannerName = "BiRRT";
            planner = RaveCreatePlanner(GetEnv(),_strRRTPlannerName);
            if( !planner ) {
                _strRRTPlannerName = "";
            }
        }

        RAVELOG_DEBUG(str(boost::format("BaseManipulation: using %s planner\n")%_strRRTPlannerName));
        return 0;
    }

    virtual bool SimulationStep(dReal fElapsedTime)
    {
        return false;
    }

    virtual bool SendCommand(std::ostream& sout, std::istream& sinput)
    {
        EnvironmentMutex::scoped_lock lock(GetEnv()->GetMutex());
        return ModuleBase::SendCommand(sout,sinput);
    }
protected:

    inline boost::shared_ptr<BaseManipulation> shared_problem() {
        return boost::dynamic_pointer_cast<BaseManipulation>(shared_from_this());
    }
    inline boost::shared_ptr<BaseManipulation const> shared_problem_const() const {
        return boost::dynamic_pointer_cast<BaseManipulation const>(shared_from_this());
    }

    bool Traj(ostream& sout, istream& sinput)
    {
        string filename; sinput >> filename;
        if( !sinput ) {
            return false;
        }
        RAVELOG_WARN("BaseManipulation Traj command is deprecated, use robot.GetController().SetPath(traj) instead\n");
        TrajectoryBasePtr ptraj = RaveCreateTrajectory(GetEnv(),"");
        char sep = ' ';
        if( filename == "sep" ) {
            sinput >> sep;
            filename = utils::GetFilenameUntilSeparator(sinput,sep);
        }

        if( filename == "stream" ) {
            // the trajectory is embedded in the stream
            RAVELOG_VERBOSE("BaseManipulation: reading trajectory from stream\n");
            ptraj->deserialize(sinput);
        }
        else {
            RAVELOG_VERBOSE(str(boost::format("BaseManipulation: reading trajectory: %s\n")%filename));
            ifstream f(filename.c_str());
            ptraj->deserialize(sinput);
        }

        bool bResetTrans = false; sinput >> bResetTrans;
        bool bResetTiming = false; sinput >> bResetTiming;

        if( bResetTrans ) {
            RAVELOG_WARN("resetting transformations of trajectory not supported\n");
        }

        if(( ptraj->GetDuration() == 0) || bResetTiming ) {
            RAVELOG_VERBOSE(str(boost::format("retiming trajectory: %f\n")%_fMaxVelMult));
            planningutils::SmoothActiveDOFTrajectory(ptraj, robot, _fMaxVelMult);
        }
        RAVELOG_VERBOSE(str(boost::format("executing traj with %d points\n")%ptraj->GetNumWaypoints()));
        if( !robot->GetController()->SetPath(ptraj) ) {
            return false;
        }
        sout << "1";
        return true;
    }

    bool SetRobotCommand(ostream& sout, istream& sinput)
    {
        string strRobotName;
        sinput >> strRobotName;
        robot = GetEnv()->GetRobot(strRobotName);
        return !!robot;
    }

    bool MoveHandStraight(ostream& sout, istream& sinput)
    {
        Vector direction = Vector(0,0,1);
        string strtrajfilename;
        bool bExecute = true;
        int minsteps = 0;
        int maxsteps = 10000;
        bool starteematrix = false;

        RobotBase::ManipulatorConstPtr pmanip = robot->GetActiveManipulator();
        Transform Tee;

        WorkspaceTrajectoryParametersPtr params(new WorkspaceTrajectoryParameters(GetEnv()));
        boost::shared_ptr<ostream> pOutputTrajStream;
        params->ignorefirstcollision = 0.1;     // 0.1**2 * 5 * 0.5 = 0.025 m
        string plannername = "workspacetrajectorytracker";
        params->_fStepLength = 0.01;
        string cmd;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "minsteps" ) {
                sinput >> minsteps;
            }
            else if( cmd == "outputtraj") {
                pOutputTrajStream = boost::shared_ptr<ostream>(&sout,utils::null_deleter());
            }
            else if( cmd == "maxsteps") {
                sinput >> maxsteps;
            }
            else if((cmd == "steplength")||(cmd == "stepsize")) {
                sinput >> params->_fStepLength;
            }
            else if( cmd == "execute" ) {
                sinput >> bExecute;
            }
            else if( cmd == "writetraj") {
                sinput >> strtrajfilename;
            }
            else if( cmd == "direction") {
                sinput >> direction.x >> direction.y >> direction.z;
                direction.normalize3();
            }
            else if( cmd == "ignorefirstcollision") {
                sinput >> params->ignorefirstcollision;
            }
            else if( cmd == "greedysearch" ) {
                sinput >> params->greedysearch;
            }
            else if( cmd == "maxdeviationangle" ) {
                sinput >> params->maxdeviationangle;
            }
            else if( cmd == "jacobian" ) {
                RAVELOG_WARN("MoveHandStraight jacobian parameter not supported anymore\n");
            }
            else if( cmd == "planner" ) {
                sinput >> plannername;
            }
            else if( cmd == "starteematrix" ) {
                TransformMatrix tm;
                starteematrix = true;
                sinput >> tm;
                Tee = tm;
            }
            else {
                RAVELOG_WARN(str(boost::format("unrecognized command: %s\n")%cmd));
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        if( maxsteps <= 0 ) {
            RAVELOG_WARN("maxsteps is not positive\n");
            return false;
        }

        params->minimumcompletetime = params->_fStepLength * minsteps;
        RAVELOG_DEBUG(str(boost::format("Starting MoveHandStraight dir=(%f,%f,%f)...")%direction.x%direction.y%direction.z));
        robot->RegrabAll();

        RobotBase::RobotStateSaver saver(robot);

        robot->SetActiveDOFs(pmanip->GetArmIndices());
        params->SetRobotActiveJoints(robot);

        CollisionOptionsStateSaver optionstate(GetEnv()->GetCollisionChecker(),GetEnv()->GetCollisionChecker()->GetCollisionOptions()|CO_ActiveDOFs,false);

        if( !starteematrix ) {
            planningutils::JitterActiveDOF(robot,100);     // try to jitter out, don't worry if it fails
            robot->GetActiveDOFValues(params->vinitialconfig);
            Tee = pmanip->GetTransform();
        }
        else {
            params->vinitialconfig.resize(0);     // set by SetRobotActiveJoints
        }

        // compute a workspace trajectory (important to do this after jittering!)
        {
            params->workspacetraj = RaveCreateTrajectory(GetEnv(),"");
            ConfigurationSpecification spec = IkParameterization::GetConfigurationSpecification(IKP_Transform6D,"quadratic");
            params->workspacetraj->Init(spec);
            vector<dReal> data(spec._vgroups[0].dof);
            IkParameterization ikparam(Tee,IKP_Transform6D);
            ikparam.GetValues(data.begin());
            params->workspacetraj->Insert(0,data);
            Tee.trans += direction*maxsteps*params->_fStepLength;
            ikparam.SetTransform6D(Tee);
            ikparam.GetValues(data.begin());
            params->workspacetraj->Insert(1,data);
            vector<dReal> maxvelocities(spec._vgroups[0].dof,1);
            vector<dReal> maxaccelerations(spec._vgroups[0].dof,5);
            planningutils::RetimeAffineTrajectory(params->workspacetraj,maxvelocities,maxaccelerations);
        }

        if( params->workspacetraj->GetDuration() <= 10*g_fEpsilon ) {
            RAVELOG_WARN("workspace traj is empty\n");
            return false;
        }

        PlannerBasePtr planner = RaveCreatePlanner(GetEnv(),plannername);
        if( !planner ) {
            RAVELOG_WARN("failed to create planner\n");
            return false;
        }

        if( !planner->InitPlan(robot, params) ) {
            RAVELOG_ERROR("InitPlan failed\n");
            return false;
        }

        TrajectoryBasePtr poutputtraj = RaveCreateTrajectory(GetEnv(),"");
        if( !planner->PlanPath(poutputtraj) ) {
            return false;
        }
        if( params->ignorefirstcollision == 0 && (RaveGetDebugLevel() & Level_VerifyPlans) ) {
            planningutils::VerifyTrajectory(params,poutputtraj);
        }
        CM::SetActiveTrajectory(robot, poutputtraj, bExecute, strtrajfilename, pOutputTrajStream,_fMaxVelMult);
        return true;
    }

    bool MoveManipulator(ostream& sout, istream& sinput)
    {
        RAVELOG_DEBUG("Starting MoveManipulator...\n");
        RobotBase::RobotStateSaver saver(robot,KinBody::Save_ActiveDOF);
        robot->SetActiveDOFs(robot->GetActiveManipulator()->GetArmIndices());
        BOOST_ASSERT(robot->GetActiveDOF()>0);
        return MoveActiveJoints(sout,sinput);
    }

    bool MoveActiveJoints(ostream& sout, istream& sinput)
    {
        string strtrajfilename;
        bool bExecute = true;
        int nMaxTries = 2;     // max tries for the planner
        boost::shared_ptr<ostream> pOutputTrajStream;

        int nMaxJitterIterations = 1000;
        RRTParametersPtr params(new RRTParameters());
        params->_minimumgoalpaths = _minimumgoalpaths;
        params->_nMaxIterations = 4000;     // max iterations before failure
        dReal jitter = 0.04;
        int usedynamicsconstraints=0;
        std::vector<dReal> vinitialconfig;
        string cmd;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "goal" ) {
                params->vgoalconfig.reserve(params->vgoalconfig.size()+robot->GetActiveDOF());
                for(int i = 0; i < robot->GetActiveDOF(); ++i) {
                    dReal f=0;
                    sinput >> f;
                    params->vgoalconfig.push_back(f);
                }
            }
            else if( cmd == "jitter" ) {
                sinput >> jitter;
            }
            else if( cmd == "goals" ) {
                size_t numgoals = 0;
                sinput >> numgoals;
                params->vgoalconfig.reserve(params->vgoalconfig.size()+numgoals*robot->GetActiveDOF());
                for(size_t i = 0; i < numgoals*robot->GetActiveDOF(); ++i) {
                    dReal f=0;
                    sinput >> f;
                    params->vgoalconfig.push_back(f);
                }
            }
            else if( cmd == "initialconfigs" ) {
                size_t num=0;
                sinput >> num;
                vinitialconfig.resize(num*robot->GetActiveDOF());
                FOREACH(itvalue, vinitialconfig) {
                    sinput >> *itvalue;
                }
            }
            else if( cmd == "outputtraj" ) {
                pOutputTrajStream = boost::shared_ptr<ostream>(&sout,utils::null_deleter());
            }
            else if( cmd == "maxiter" ) {
                sinput >> params->_nMaxIterations;
            }
            else if( cmd == "execute" ) {
                sinput >> bExecute;
            }
            else if( cmd == "writetraj" ) {
                sinput >> strtrajfilename;
            }
            else if( cmd == "steplength" ) {
                sinput >> params->_fStepLength;
            }
            else if( cmd == "maxtries" ) {
                sinput >> nMaxTries;
            }
            else if( cmd == "postprocessingparameters" ) {
                if( !getline(sinput, params->_sPostProcessingParameters) ) {
                    return false;
                }
                boost::trim(params->_sPostProcessingParameters);
            }
            else if( cmd == "postprocessingplanner" ) {
                if( !getline(sinput, params->_sPostProcessingPlanner) ) {
                    return false;
                }
                boost::trim(params->_sPostProcessingPlanner);
            }
            else if( cmd == "usedynamicsconstraints" ) {
                sinput >> usedynamicsconstraints;
            }
            else {
                RAVELOG_WARN(str(boost::format("unrecognized command: %s\n")%cmd));
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        if( params->vgoalconfig.size() == 0 ) {
            return false;
        }
        RobotBase::RobotStateSaver saver(robot);
        params->SetRobotActiveJoints(robot);
        if( vinitialconfig.size() > 0 ) {
            params->vinitialconfig.swap(vinitialconfig);
        }

        if( usedynamicsconstraints ) {
            // use dynamics constraints, so remove the old path constraint function
            params->_checkpathconstraintsfn.clear();
            std::list<KinBodyPtr> listCheckBodies;
            listCheckBodies.push_back(robot);
            planningutils::DynamicsCollisionConstraintPtr dynamics(new planningutils::DynamicsCollisionConstraint(params, listCheckBodies, 0xffffffff));
            params->_checkpathvelocityconstraintsfn = boost::bind(&planningutils::DynamicsCollisionConstraint::Check,dynamics,_1,_2,_3,_4,_5,_6,_7,_8);
        }
        if( _sPostProcessingParameters.size() > 0 ) {
            params->_sPostProcessingParameters = _sPostProcessingParameters;
        }

        CollisionOptionsStateSaver optionstate(GetEnv()->GetCollisionChecker(),GetEnv()->GetCollisionChecker()->GetCollisionOptions()|CO_ActiveDOFs,false);

        // make sure the initial and goal configs are not in collision
        vector<dReal> vonegoal(robot->GetActiveDOF());
        vector<dReal> voriggoalconfig; voriggoalconfig.reserve(params->vgoalconfig.size());
        std::vector<uint8_t> vgoalchanged; vgoalchanged.reserve(params->vgoalconfig.size()/robot->GetActiveDOF());

        size_t writeindex=0;
        for(size_t i = 0; i < params->vgoalconfig.size(); i += robot->GetActiveDOF()) {
            std::copy(params->vgoalconfig.begin()+i,params->vgoalconfig.begin()+i+robot->GetActiveDOF(),vonegoal.begin());
            robot->SetActiveDOFValues(vonegoal, true);
            robot->GetActiveDOFValues(vonegoal);
            uint8_t goalchanged = 0;
            switch( planningutils::JitterActiveDOF(robot,nMaxJitterIterations,jitter) ) {
            case 0:
                RAVELOG_WARN(str(boost::format("jitter failed %d\n")%i));
                continue;
            case 1:
                goalchanged = 1;
                break;
            }
            voriggoalconfig.insert(voriggoalconfig.end(),vonegoal.begin(),vonegoal.end());
            if( goalchanged ) {
                robot->GetActiveDOFValues(vonegoal);
            }
            vgoalchanged.push_back(goalchanged);
            std::copy(vonegoal.begin(),vonegoal.end(),params->vgoalconfig.begin()+writeindex);
            writeindex += robot->GetActiveDOF();
        }
        if( writeindex == 0 ) {
            return false;
        }
        params->vgoalconfig.resize(writeindex);
        robot->SetActiveDOFValues(params->vinitialconfig);

        vector<dReal> vinsertconfiguration; // configuration to add at the beginning of the trajectory, usually it is in collision
        // jitter again for initial collision
        switch( planningutils::JitterActiveDOF(robot,nMaxJitterIterations,jitter) ) {
        case 0:
            RAVELOG_WARN("jitter failed for initial\n");
            return false;
        case 1:
            RAVELOG_DEBUG("original robot position in collision, so jittered out of it\n");
            vinsertconfiguration = params->vinitialconfig;
            robot->GetActiveDOFValues(params->vinitialconfig);
            break;
        }

        PlannerBasePtr rrtplanner = RaveCreatePlanner(GetEnv(),_strRRTPlannerName);
        if( !rrtplanner ) {
            RAVELOG_ERROR("failed to create BiRRTs\n");
            return false;
        }

        TrajectoryBasePtr ptraj = RaveCreateTrajectory(GetEnv(),"");

        RAVELOG_DEBUG("starting planning\n");
        bool bSuccess = false;
        for(int itry = 0; itry < nMaxTries; ++itry) {
            if( !rrtplanner->InitPlan(robot, params) ) {
                RAVELOG_ERROR("InitPlan failed\n");
                return false;
            }

            if( !rrtplanner->PlanPath(ptraj) ) {
                RAVELOG_WARN("PlanPath failed\n");
            }
            else {
                bSuccess = true;
                RAVELOG_DEBUG("finished planning\n");
                break;
            }
        }

        if( !bSuccess ) {
            return false;
        }
        if( RaveGetDebugLevel() & Level_VerifyPlans ) {
            planningutils::VerifyTrajectory(params, ptraj);
        }
        if( vinsertconfiguration.size() > 0 ) {
            planningutils::InsertActiveDOFWaypointWithRetiming(0,vinsertconfiguration,vector<dReal>(), ptraj, robot, _fMaxVelMult);
        }

        if( jitter > 0 ) {
            // could have returned a jittered goal different from the original goals
            try {
                stringstream soutput, sinput;
                sinput << "GetGoalIndex";
                rrtplanner->SendCommand(soutput,sinput);

                int goalindex = -1;
                soutput >> goalindex;
                if( goalindex >= 0 && goalindex < (int)vgoalchanged.size() && vgoalchanged.at(goalindex) ) {
                    std::copy(voriggoalconfig.begin()+goalindex*robot->GetActiveDOF(),voriggoalconfig.begin()+(goalindex+1)*robot->GetActiveDOF(),vonegoal.begin());
                    planningutils::InsertActiveDOFWaypointWithRetiming(ptraj->GetNumWaypoints(),vonegoal,vector<dReal>(), ptraj, robot, _fMaxVelMult);
                }
            }
            catch(const std::exception& ex) {
                RAVELOG_WARN(str(boost::format("planner %s does not support GetGoalIndex command necessary for determining what goal it chose! %s")%rrtplanner->GetXMLId()%ex.what()));
            }
        }

        CM::SetActiveTrajectory(robot, ptraj, bExecute, strtrajfilename, pOutputTrajStream,_fMaxVelMult);
        return true;
    }

    bool _MoveToHandPosition(ostream& sout, istream& sinput)
    {
        RAVELOG_DEBUG("Starting MoveToHandPosition...\n");
        RobotBase::ManipulatorConstPtr pmanip = robot->GetActiveManipulator();
        std::list<IkParameterization> listgoals;

        string strtrajfilename;
        bool bExecute = true;
        boost::shared_ptr<ostream> pOutputTrajStream;

        Vector vconstraintaxis, vconstraintpos;
        int affinedofs = 0;
        int nSeedIkSolutions = 8;     // no extra solutions
        int nMaxTries = 3;     // max tries for the planner

        RRTParametersPtr params(new RRTParameters());
        params->_minimumgoalpaths = _minimumgoalpaths;
        params->_nMaxIterations = 4000;
        int nMaxJitterIterations = 1000;
        // constraint stuff
        boost::array<double,6> vconstraintfreedoms = { { 0,0,0,0,0,0}};
        Transform tConstraintTargetWorldFrame, tConstraintTaskFrame;
        double constrainterrorthresh=0;
        int goalsamples = 40;
        string cmd;
        dReal jitter = 0.03;
        dReal jitterikparam = 0;
        dReal goalsampleprob = 0.1;
        int nGoalMaxTries=10;
        std::vector<dReal> vinitialconfig;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "translation" ) {
                Vector trans;
                sinput >> trans.x >> trans.y >> trans.z;
                listgoals.push_back(IkParameterization());
                listgoals.back().SetTranslation3D(trans);
            }
            else if( cmd == "rotation" ) {
                Vector q;
                sinput >> q.x >> q.y >> q.z >> q.w;
                listgoals.push_back(IkParameterization());
                listgoals.back().SetRotation3D(q);
            }
            else if( cmd == "outputtraj" ) {
                pOutputTrajStream = boost::shared_ptr<ostream>(&sout,utils::null_deleter());
            }
            else if( cmd == "matrix" ) {
                TransformMatrix m;
                sinput >> m;
                listgoals.push_back(IkParameterization(Transform(m)));
            }
            else if( cmd == "goalsamples" ) {
                sinput >> goalsamples;
            }
            else if( cmd == "matrices" ) {
                TransformMatrix m;
                int num = 0;
                sinput >> num;
                while(num-->0) {
                    sinput >> m;
                    listgoals.push_back(IkParameterization(Transform(m)));
                }
            }
            else if( cmd == "pose" ) {
                Transform t;
                sinput >> t;
                listgoals.push_back(IkParameterization(t));
            }
            else if( cmd == "poses" ) {
                int num = 0;
                sinput >> num;
                while(num-->0) {
                    Transform t;
                    sinput >> t;
                    listgoals.push_back(IkParameterization(t));
                }
            }
            else if( cmd == "ikparams" ) {
                int num = 0;
                sinput >> num;
                while(num-->0) {
                    IkParameterization ikparam;
                    sinput >> ikparam;
                    listgoals.push_back(ikparam);
                }
            }
            else if( cmd == "ikparam" ) {
                IkParameterization ikparam;
                sinput >> ikparam;
                listgoals.push_back(ikparam);
            }
            else if( cmd == "affinedofs" ) {
                sinput >> affinedofs;
            }
            else if( cmd == "maxiter" ) {
                sinput >> params->_nMaxIterations;
            }
            else if( cmd == "maxtries" ) {
                sinput >> nMaxTries;
            }
            else if( cmd == "execute" ) {
                sinput >> bExecute;
            }
            else if( cmd == "writetraj" ) {
                sinput >> strtrajfilename;
            }
            else if( cmd == "seedik" ) {
                sinput >> nSeedIkSolutions;
            }
            else if( cmd == "steplength" ) {
                sinput >> params->_fStepLength;
            }
            else if( cmd == "constraintfreedoms" ) {
                FOREACH(it,vconstraintfreedoms) {
                    sinput >> *it;
                }
            }
            else if( cmd == "constraintmatrix" ) {
                TransformMatrix m; sinput >> m; tConstraintTargetWorldFrame = m;
            }
            else if( cmd == "constraintpose" ) {
                sinput >> tConstraintTargetWorldFrame;
            }
            else if( cmd == "constrainttaskmatrix" ) {
                TransformMatrix m; sinput >> m; tConstraintTaskFrame = m;
            }
            else if( cmd == "constrainttaskpose" ) {
                sinput >> tConstraintTaskFrame;
            }
            else if( cmd == "constrainterrorthresh" ) {
                sinput >> constrainterrorthresh;
            }
            else if( cmd == "minimumgoalpaths" ) {
                sinput >> params->_minimumgoalpaths;
            }
            else if( cmd == "postprocessingparameters" ) {
                if( !getline(sinput, params->_sPostProcessingParameters) ) {
                    return false;
                }
                boost::trim(params->_sPostProcessingParameters);
            }
            else if( cmd == "postprocessingplanner" ) {
                if( !getline(sinput, params->_sPostProcessingPlanner) ) {
                    return false;
                }
                boost::trim(params->_sPostProcessingPlanner);
            }
            else if( cmd == "jitter" ) {
                sinput >> jitter;
            }
            else if( cmd == "jitterikparam" || cmd == "jittergoal" ) {
                sinput >> jitterikparam;
            }
            else if( cmd == "goalsampleprob" ) {
                sinput >> goalsampleprob;
            }
            else if( cmd == "goalmaxtries" ) {
                sinput >> nGoalMaxTries;
            }
            else if( cmd == "initialconfigs" ) {
                size_t num=0;
                sinput >> num;
                vinitialconfig.resize(num*pmanip->GetArmIndices().size());
                FOREACH(itvalue, vinitialconfig) {
                    sinput >> *itvalue;
                }
            }
            else {
                RAVELOG_WARN(str(boost::format("unrecognized command: %s\n")%cmd));
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        robot->RegrabAll();
        RobotBase::RobotStateSaver saver(robot);
        robot->SetActiveDOFs(pmanip->GetArmIndices(), affinedofs);
        params->SetRobotActiveJoints(robot);
        if( vinitialconfig.size() > 0 ) {
            params->vinitialconfig.swap(vinitialconfig);
        }
        if( _sPostProcessingParameters.size() > 0 ) {
            params->_sPostProcessingParameters = _sPostProcessingParameters;
        }

        CollisionOptionsStateSaver optionstate(GetEnv()->GetCollisionChecker(),GetEnv()->GetCollisionChecker()->GetCollisionOptions()|CO_ActiveDOFs,false);

        if( constrainterrorthresh > 0 ) {
            RAVELOG_DEBUG("setting jacobian constraint function in planner parameters\n");
            robot->SetActiveDOFValues(params->vinitialconfig); // have to set the initial configuraiton!
            boost::shared_ptr<CM::GripperJacobianConstrains<double> > pconstraints(new CM::GripperJacobianConstrains<double>(robot->GetActiveManipulator(),tConstraintTargetWorldFrame,tConstraintTaskFrame, vconstraintfreedoms,constrainterrorthresh));
            pconstraints->_distmetricfn = params->_distmetricfn;
            params->_neighstatefn = boost::bind(&CM::GripperJacobianConstrains<double>::RetractionConstraint,pconstraints,_1,_2);
            // use linear interpolation!
            params->_sPostProcessingPlanner = "shortcut_linear";
            params->_sPostProcessingParameters ="<_nmaxiterations>100</_nmaxiterations><_postprocessing planner=\"lineartrajectoryretimer\"></_postprocessing>";
            vector<dReal> vdelta(params->vinitialconfig.size(),0);
            if( !params->_neighstatefn(params->vinitialconfig,vdelta,0)) {
                throw OPENRAVE_EXCEPTION_FORMAT0("initial configuration does not follow constraints",ORE_InconsistentConstraints);
            }
        }

        robot->SetActiveDOFs(pmanip->GetArmIndices(), 0);

        vector<dReal> vgoal;
        planningutils::ManipulatorIKGoalSampler goalsampler(pmanip, listgoals,goalsamples,nGoalMaxTries);
        goalsampler.SetJitter(jitterikparam);
        params->vgoalconfig.reserve(nSeedIkSolutions*robot->GetActiveDOF());
        while(nSeedIkSolutions > 0) {
            if( goalsampler.Sample(vgoal) ) {
                if(constrainterrorthresh > 0 ) {
                    robot->SetActiveDOFValues(vgoal);
                    switch( planningutils::JitterActiveDOF(robot,nMaxJitterIterations,jitter,params->_neighstatefn) ) {
                    case 0:
                        RAVELOG_DEBUG("constraint function failed\n");
                        continue;
                    case 1:
                        robot->GetActiveDOFValues(vgoal);
                        break;
                    }
                }
                params->vgoalconfig.insert(params->vgoalconfig.end(), vgoal.begin(), vgoal.end());
                --nSeedIkSolutions;
            }
            else {
                --nSeedIkSolutions;
            }
        }
        goalsampler.SetSamplingProb(goalsampleprob);
        params->_samplegoalfn = boost::bind(&planningutils::ManipulatorIKGoalSampler::Sample,&goalsampler,_1);

        if( params->vgoalconfig.size() == 0 ) {
            RAVELOG_WARN("failed to find goal\n");
            return false;
        }

        // restore
        robot->SetActiveDOFValues(params->vinitialconfig);

        vector<dReal> vinsertconfiguration; // configuration to add at the beginning of the trajectory, usually it is in collision
        // jitter again for initial collision
        switch( planningutils::JitterActiveDOF(robot,nMaxJitterIterations,jitter,constrainterrorthresh > 0 ? params->_neighstatefn : PlannerBase::PlannerParameters::NeighStateFn()) ) {
        case 0:
            RAVELOG_WARN("jitter failed for initial\n");
            return false;
        case 1:
            RAVELOG_DEBUG("original robot position in collision, so jittered out of it\n");
            vinsertconfiguration = params->vinitialconfig;
            robot->GetActiveDOFValues(params->vinitialconfig);
            break;
        }

        PlannerBasePtr rrtplanner = RaveCreatePlanner(GetEnv(),_strRRTPlannerName);
        if( !rrtplanner ) {
            RAVELOG_ERROR("failed to create BiRRTs\n");
            return false;
        }

        bool bSuccess = false;
        TrajectoryBasePtr ptraj = RaveCreateTrajectory(GetEnv(),"");
        RAVELOG_DEBUG("starting planning\n");

        for(int iter = 0; iter < nMaxTries; ++iter) {
            if( !rrtplanner->InitPlan(robot, params) ) {
                RAVELOG_ERROR("InitPlan failed\n");
                return false;
            }

            if( rrtplanner->PlanPath(ptraj) ) {
                bSuccess = true;
                RAVELOG_DEBUG("finished planning\n");
                break;
            }
            else {
                RAVELOG_WARN("PlanPath failed\n");
            }
        }

        if( !bSuccess ) {
            return false;
        }
        if( RaveGetDebugLevel() & Level_VerifyPlans ) {
            planningutils::VerifyTrajectory(params, ptraj);
        }

        if( vinsertconfiguration.size() > 0 ) {
            planningutils::InsertActiveDOFWaypointWithRetiming(0,vinsertconfiguration,vector<dReal>(), ptraj, robot, _fMaxVelMult);
        }

        if( jitterikparam > 0 ) {
            // could have returned a jittered goal different from the original goals
            try {
                stringstream soutput, sinput;
                sinput << "GetGoalIndex";
                rrtplanner->SendCommand(soutput,sinput);
            }
            catch(const std::exception& ex) {
                RAVELOG_WARN(str(boost::format("planner %s does not support GetGoalIndex command necessary for determining what goal it chose! %s")%rrtplanner->GetXMLId()%ex.what()));
            }
        }

        CM::SetActiveTrajectory(robot, ptraj, bExecute, strtrajfilename, pOutputTrajStream,_fMaxVelMult);
        sout << "1";
        return true;
    }

    bool MoveUnsyncJoints(ostream& sout, istream& sinput)
    {
        string strplanner = "BasicRRT";
        string strsavetraj;
        string cmd;
        vector<int> vhandjoints;
        vector<dReal> vhandgoal;
        bool bExecute = true;
        boost::shared_ptr<ostream> pOutputTrajStream;
        int nMaxTries=1;
        int maxdivision=10;
        int nMaxJitterIterations = 1000;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "writetraj" ) {
                sinput >> strsavetraj;
            }
            else if( cmd == "outputtraj" ) {
                pOutputTrajStream = boost::shared_ptr<ostream>(&sout,utils::null_deleter());
            }
            else if( cmd == "handjoints" ) {
                int dof = 0;
                sinput >> dof;
                if( !sinput ||( dof == 0) ) {
                    return false;
                }
                vhandjoints.resize(dof);
                vhandgoal.resize(dof);
                FOREACH(it, vhandgoal) {
                    sinput >> *it;
                }
                FOREACH(it, vhandjoints) {
                    sinput >> *it;
                }
            }
            else if( cmd == "planner" ) {
                sinput >> strplanner;
            }
            else if( cmd == "execute" ) {
                sinput >> bExecute;
            }
            else if( cmd == "maxtries" ) {
                sinput >> nMaxTries;
            }
            else if( cmd == "maxdivision" ) {
                sinput >> maxdivision;
            }
            else {
                RAVELOG_WARN(str(boost::format("unrecognized command: %s\n")%cmd));
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        RobotBase::RobotStateSaver saver(robot);
        uint32_t starttime = utils::GetMilliTime();
        if( planningutils::JitterActiveDOF(robot,nMaxJitterIterations) == 0 ) {
            RAVELOG_WARN("failed to jitter robot out of collision\n");
        }

        TrajectoryBasePtr ptraj = RaveCreateTrajectory(GetEnv(),"");

        bool bSuccess = false;
        for(int itry = 0; itry < nMaxTries; ++itry) {
            if( CM::MoveUnsync::_MoveUnsyncJoints(GetEnv(), robot, ptraj, vhandjoints, vhandgoal, strplanner,maxdivision) ) {
                bSuccess = true;
                break;
            }
        }
        if( !bSuccess ) {
            return false;
        }

        BOOST_ASSERT(ptraj->GetNumWaypoints() > 0);

        bool bExecuted = CM::SetActiveTrajectory(robot, ptraj, bExecute, strsavetraj, pOutputTrajStream,_fMaxVelMult);
        sout << (int)bExecuted << " ";
        sout << (utils::GetMilliTime()-starttime)/1000.0f << " ";
        vector<dReal> q;
        ptraj->GetWaypoint(-1,q,robot->GetActiveConfigurationSpecification());
        FOREACH(it, q) {
            sout << *it << " ";
        }
        return true;
    }

    bool JitterActive(ostream& sout, istream& sinput)
    {
        RAVELOG_DEBUG("Starting JitterActive...\n");
        bool bExecute = true, bOutputFinal=false;
        boost::shared_ptr<ostream> pOutputTrajStream;
        string cmd;
        int nMaxJitterIterations=5000;
        dReal fJitter=0.03f;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "execute" ) {
                sinput >> bExecute;
            }
            else if( cmd == "maxiter" ) {
                sinput >> nMaxJitterIterations;
            }
            else if( cmd == "jitter" ) {
                sinput >> fJitter;
            }
            else if( cmd == "outputtraj" ) {
                pOutputTrajStream = boost::shared_ptr<ostream>(&sout,utils::null_deleter());
            }
            else if( cmd == "outputfinal" ) {
                bOutputFinal = true;
            }
            else {
                RAVELOG_WARN(str(boost::format("unrecognized command: %s\n")%cmd));
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        RobotBase::RobotStateSaver saver(robot);
        TrajectoryBasePtr ptraj = RaveCreateTrajectory(GetEnv(),"");
        ptraj->Init(robot->GetActiveConfigurationSpecification());

        vector<dReal> vvalues;
        robot->GetActiveDOFValues(vvalues);
        ptraj->Insert(0,vvalues);
        switch( planningutils::JitterActiveDOF(robot,nMaxJitterIterations,fJitter) ) {
        case 0:
            RAVELOG_WARN("could not jitter out of collision\n");
            return false;
        case 1:
            robot->GetActiveDOFValues(vvalues);
            ptraj->Insert(1,vvalues);
        default:
            break;
        }

        if( bOutputFinal ) {
            FOREACH(itq,vvalues) {
                sout << *itq << " ";
            }
        }

        CM::SetActiveTrajectory(robot, ptraj, bExecute, "", pOutputTrajStream,_fMaxVelMult);
        return true;
    }

    bool GrabBody(ostream& sout, istream& sinput)
    {
        RAVELOG_WARN("BaseManipulation GrabBody command is deprecated. Use Robot::Grab (11/03/07)\n");
        KinBodyPtr ptarget;

        string cmd;
        while(!sinput.eof()) {
            sinput >> cmd;
            if( !sinput ) {
                break;
            }
            std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

            if( cmd == "name" ) {
                string name;
                sinput >> name;
                ptarget = GetEnv()->GetKinBody(name);
            }
            else {
                break;
            }

            if( !sinput ) {
                RAVELOG_ERROR(str(boost::format("failed processing command %s\n")%cmd));
                return false;
            }
        }

        if(!ptarget) {
            RAVELOG_ERROR("ERROR Manipulation::GrabBody - Invalid body name.\n");
            return false;
        }

        RAVELOG_DEBUG(str(boost::format("robot %s:%s grabbing body %s...\n")%robot->GetName()%robot->GetActiveManipulator()->GetEndEffector()->GetName()%ptarget->GetName()));
        robot->Grab(ptarget);
        return true;
    }

    bool ReleaseAll(ostream& sout, istream& sinput)
    {
        RAVELOG_WARN("BaseManipulation ReleaseAll command is deprecated. Use Robot::ReleaseAllGrabbed (11/03/07)\n");
        if( !!robot ) {
            RAVELOG_DEBUG("Releasing all bodies\n");
            robot->ReleaseAllGrabbed();
        }
        return true;
    }

protected:
    bool SetMinimumGoalPathsCommand(ostream& sout, istream& sinput)
    {
        sinput >> _minimumgoalpaths;
        BOOST_ASSERT(_minimumgoalpaths>=0);
        return !!sinput;
    }

    bool SetPostProcessingCommand(ostream& sout, istream& sinput)
    {
        if( !getline(sinput, _sPostProcessingParameters) ) {
            return false;
        }
        return !!sinput;
    }

    RobotBasePtr robot;
    string _strRRTPlannerName;
    dReal _fMaxVelMult;
    int _minimumgoalpaths;
    string _sPostProcessingParameters;
};

ModuleBasePtr CreateBaseManipulation(EnvironmentBasePtr penv) {
    return ModuleBasePtr(new BaseManipulation(penv));
}

max_issues_repo_name: jdsika/holy

max_forks_repo_name: jdsika/holy