#!/usr/bin/env python
# -*- encoding: utf-8 -*-
"""
    模型: bi-lstm + crf
"""
import math
import numpy as np
from tqdm import tqdm
import tensorflow as tf
from tensorflow.contrib import rnn
from utils import uniform_tensor, get_sequence_actual_length, \
    zero_nil_slot, shuffle_matrix

def get_activation(activation=None):
    """
    Get activation function accord to the parameter 'activation'
    Args:
        activation: str: 激活函数的名称
    Return:
        激活函数
    """
    if activation is None:
        return None
    elif activation == 'tanh':
        return tf.nn.tanh
    elif activation == 'relu':
        return tf.nn.relu
    elif activation == 'softmax':
        return tf.nn.softmax
    elif activation == 'sigmoid':
        return tf.sigmoid
    else:
        raise Exception('Unknow activation function: %s' % activation)

class SequenceLabelingModel(object):

    def __init__(self, sequence_length, nb_classes, nb_hidden=512, num_layers=1,
                 rnn_dropout=0., feature_names=None, feature_init_weight_dict=None,
                 feature_weight_shape_dict=None, feature_weight_dropout_dict=None,
                 dropout_rate=0., use_crf=True, path_model=None, nb_epoch=200,
                 batch_size=128, train_max_patience=10, l2_rate=0.01, rnn_unit='lstm',
                 learning_rate=0.001, clip=None, use_char_feature=False, word_length=None,
                 conv_filter_size_list=None, conv_filter_len_list=None, cnn_dropout_rate=0.):
        """
        Args:
          sequence_length: int, 输入序列的padding后的长度
          nb_classes: int, 标签类别数量
          nb_hidden: int, lstm/gru层的结点数
          num_layers: int, lstm/gru层数
          rnn_dropout: lstm层的dropout值

          feature_names: list of str, 特征名称集合
          feature_init_weight_dict: dict, 键:特征名称, 值:np,array, 特征的初始化权重字典
          feature_weight_shape_dict: dict，特征embedding权重的shape，键:特征名称, 值: shape(tuple)。
          feature_weight_dropout_dict: feature name to float, feature weights dropout rate

          dropout: float, dropout rate
          use_crf: bool, 标示是否使用crf层
          path_model: str, 模型保存的路径
          nb_epoch: int, 训练最大迭代次数
          batch_size: int
          train_max_patience: int, 在dev上的loss对于train_max_patience次没有提升，则early stopping

          l2_rate: float

          rnn_unit: str, lstm or gru
          learning_rate: float, default is 0.001
          clip: None or float, gradients clip

          use_char_feature: bool,是否使用字符特征
          word_length: int, 单词长度
        """
        self._sequence_length = sequence_length
        self._nb_classes = nb_classes
        self._nb_hidden = nb_hidden
        self._num_layers = num_layers
        self._rnn_dropout = rnn_dropout

        self._feature_names = feature_names
        self._feature_init_weight_dict = feature_init_weight_dict if \
            feature_init_weight_dict else dict()
        self._feature_weight_shape_dict = feature_weight_shape_dict
        self._feature_weight_dropout_dict = feature_weight_dropout_dict

        self._dropout_rate = dropout_rate
        self._use_crf = use_crf

        self._path_model = path_model
        self._nb_epoch = nb_epoch
        self._batch_size = batch_size
        self._train_max_patience = train_max_patience

        self._l2_rate = l2_rate
        self._rnn_unit = rnn_unit
        self._learning_rate = learning_rate
        self._clip = clip

        self._use_char_feature = use_char_feature
        self._word_length = word_length
        self._conv_filter_len_list = conv_filter_len_list
        self._conv_filter_size_list = conv_filter_size_list
        self._cnn_dropout_rate = cnn_dropout_rate
        self._get_rnn_unit = self.get_rnn_unit
        assert len(feature_names) == len(list(set(feature_names))), \
            'duplication of feature names!'

        # init ph, weights and dropout rate
        self.input_feature_ph_dict = dict()
        self.weight_dropout_ph_dict = dict()
        self.feature_weight_dict = dict()
        self.nil_vars = set()
        self.dropout_rate_ph = tf.placeholder(tf.float32, name='dropout_rate_ph') ##
        self.rnn_dropout_rate_ph = tf.placeholder(tf.float32, name='rnn_dropout_rate_ph') ##
        # label ph
        self.input_label_ph = tf.placeholder(
            dtype=tf.int32, shape=[None, self._sequence_length], name='input_label_ph') ##
        if self._use_char_feature:
            self.cnn_dropout_rate_ph = tf.placeholder(tf.float32, name='cnn_dropout_rate_ph')

        self.build_model()

    def build_model(self):
        for feature_name in self._feature_names:

            # input ph
            self.input_feature_ph_dict[feature_name] = tf.placeholder(
                dtype=tf.int32, shape=[None, self._sequence_length],
                name='input_feature_ph_%s' % feature_name) ##

            # dropout rate ph
            self.weight_dropout_ph_dict[feature_name] = tf.placeholder(
                tf.float32, name='dropout_ph_%s' % feature_name) ##

            # init feature weights, 初始化未指定的
            if feature_name not in self._feature_init_weight_dict:
                feature_weight = uniform_tensor(
                    shape=self._feature_weight_shape_dict[feature_name],
                    name='f_w_%s' % feature_name)
                self.feature_weight_dict[feature_name] = tf.Variable(
                    initial_value=feature_weight, name='feature_weigth_%s' % feature_name)
            else:
                self.feature_weight_dict[feature_name] = tf.Variable(
                    initial_value=self._feature_init_weight_dict[feature_name],
                    name='feature_weight_%s' % feature_name)
                self.nil_vars.add(self.feature_weight_dict[feature_name].name)

            # init dropout rate, 初始化未指定的
            if feature_name not in self._feature_weight_dropout_dict:
                self._feature_weight_dropout_dict[feature_name] = 0.

        # init embeddings
        self.embedding_features = []
        for feature_name in self._feature_names:
            embedding_feature = tf.nn.dropout(tf.nn.embedding_lookup(
                self.feature_weight_dict[feature_name],
                ids=self.input_feature_ph_dict[feature_name],
                name='embedding_feature_%s' % feature_name),
                keep_prob=1.-self.weight_dropout_ph_dict[feature_name],
                name='embedding_feature_dropout_%s' % feature_name)
            self.embedding_features.append(embedding_feature)

        # concat all features
        input_features = self.embedding_features[0] if len(self.embedding_features) == 1 \
            else tf.concat(values=self.embedding_features, axis=2, name='input_features')
        if self._use_char_feature:
            input_features = tf.concat([input_features, couv_feature_char], axis=-1)

        # multi bi-lstm layer
        _fw_cells = []
        _bw_cells = []
        for _ in range(self._num_layers):
            fw, bw = self._get_rnn_unit(self._rnn_unit)
            _fw_cells.append(tf.nn.rnn_cell.DropoutWrapper(fw, output_keep_prob=1-self.rnn_dropout_rate_ph))
            _bw_cells.append(tf.nn.rnn_cell.DropoutWrapper(bw, output_keep_prob=1-self.rnn_dropout_rate_ph))
        fw_cell = tf.nn.rnn_cell.MultiRNNCell(_fw_cells)
        bw_cell = tf.nn.rnn_cell.MultiRNNCell(_bw_cells)

        # 计算self.input_features[feature_names[0]]的实际长度(0为padding值)
        self.sequence_actual_length = get_sequence_actual_length(  # 每个句子的实际长度
            self.input_feature_ph_dict[self._feature_names[0]])
        rnn_outputs, _ = tf.nn.bidirectional_dynamic_rnn(
            fw_cell, bw_cell, input_features, scope='bi-lstm',
            dtype=tf.float32, sequence_length=self.sequence_actual_length)
        # shape = [batch_size, max_len, nb_hidden*2]
        lstm_output = tf.nn.dropout(
            tf.concat(rnn_outputs, axis=2, name='lstm_output'),
            keep_prob=1.-self.dropout_rate_ph, name='lstm_output_dropout')

        # softmax
        hidden_size = int(lstm_output.shape[-1])
        self.outputs = tf.reshape(lstm_output, [-1, hidden_size], name='outputs')
        self.softmax_w = tf.get_variable('softmax_w', [hidden_size, self._nb_classes])
        self.softmax_b = tf.get_variable('softmax_b', [self._nb_classes])
        self.logits = tf.reshape(
            tf.matmul(self.outputs, self.softmax_w) + self.softmax_b,
            shape=[-1, self._sequence_length, self._nb_classes], name='logits')

        # 计算loss
        self.loss = self.compute_loss()
        self.l2_loss = self._l2_rate * (tf.nn.l2_loss(self.softmax_w) + tf.nn.l2_loss(self.softmax_b))

        self.total_loss = self.loss + self.l2_loss

        # train op
        #optimizer = tf.train.AdamOptimizer(learning_rate=self._learning_rate)

        LEARNING_RATE_BASE = 0.01
        LEARNING_RATE_DECAY = 0.9
        LEARNING_RATE_STEP = 1
        gloabl_steps = tf.Variable(0, trainable=False)

        learning_rate = tf.train.exponential_decay(LEARNING_RATE_BASE
                                                   , gloabl_steps,
                                                   LEARNING_RATE_STEP,
                                                   LEARNING_RATE_DECAY,
                                                   staircase=True)

        optimizer = tf.train.AdamOptimizer(learning_rate=learning_rate)

        grads_and_vars = optimizer.compute_gradients(self.total_loss)
        nil_grads_and_vars = []
        for g, v in grads_and_vars:
            if v.name in self.nil_vars:
                nil_grads_and_vars.append((zero_nil_slot(g), v))
            else:
                nil_grads_and_vars.append((g, v))

        global_step = tf.Variable(0, name='global_step', trainable=False)
        if self._clip:
            # clip by global norm
            gradients, variables = zip(*nil_grads_and_vars)
            gradients, _ = tf.clip_by_global_norm(gradients, self._clip)
            self.train_op = optimizer.apply_gradients(
                zip(gradients, variables), name='train_op', global_step=global_step)
        else:
            self.train_op = optimizer.apply_gradients(
                nil_grads_and_vars, name='train_op', global_step=global_step)

        # TODO sess, visible_device_list待修改
        gpu_options = tf.GPUOptions(visible_device_list='0', allow_growth=True)
        self.sess = tf.Session(config=tf.ConfigProto(gpu_options=gpu_options))

        # init all variable
        init = tf.global_variables_initializer()
        self.sess.run(init)

    def get_rnn_unit(self, rnn_unit):
        if rnn_unit == 'lstm':
            fw_cell = rnn.BasicLSTMCell(self._nb_hidden, forget_bias=1., state_is_tuple=True)
            bw_cell = rnn.BasicLSTMCell(self._nb_hidden, forget_bias=1., state_is_tuple=True)
        elif rnn_unit == 'gru':
            fw_cell = rnn.GRUCell(self._nb_hidden)
            bw_cell = rnn.GRUCell(self._nb_hidden)
        else:
            raise ValueError('rnn_unit must in (lstm, gru)!')
        return fw_cell, bw_cell

    def fit(self, data_dict, dev_size=0.15, seed=1337):
        """
        训练
        Args:
            data_dict: dict, 键: 特征名(or 'label'), 值: np.array
            dev_size: float, 开发集所占的比例，default is 0.2

            batch_size: int
            seed: int, for shuffle data
        """
        data_train_dict, data_dev_dict = self.split_train_dev(data_dict, dev_size=dev_size)
        self.saver = tf.train.Saver()  # save model
        train_data_count = data_train_dict['label'].shape[0]
        nb_train = int(math.ceil(train_data_count / float(self._batch_size)))
        min_dev_loss = 1000  # 全局最小dev loss, for early stopping)
        current_patience = 0  # for early stopping
        for step in range(self._nb_epoch):
            print('Epoch %d / %d:' % (step+1, self._nb_epoch))

            # shuffle train data
            data_list = [data_train_dict['label']]
            [data_list.append(data_train_dict[name]) for name in self._feature_names]
            shuffle_matrix(*data_list, seed=seed)

            # train
            train_loss, l2_loss = 0., 0.
            for i in tqdm(range(nb_train)):
                feed_dict = dict()
                batch_indices = np.arange(i * self._batch_size, (i + 1) * self._batch_size) \
                    if (i+1)*self._batch_size <= train_data_count else \
                    np.arange(i * self._batch_size, train_data_count)
                # feature feed and dropout feed
                for feature_name in self._feature_names:  # features
                    # feature
                    batch_data = data_train_dict[feature_name][batch_indices] ##
                    item = {self.input_feature_ph_dict[feature_name]: batch_data}
                    feed_dict.update(item)
                    # dropout
                    dropout_rate = self._feature_weight_dropout_dict[feature_name] ##
                    item = {self.weight_dropout_ph_dict[feature_name]: dropout_rate}
                    feed_dict.update(item)

                feed_dict.update(
                    {
                        self.dropout_rate_ph: self._dropout_rate, ##
                        self.rnn_dropout_rate_ph: self._rnn_dropout, ##
                    })

                # label feed
                batch_label = data_train_dict['label'][batch_indices] ##
                feed_dict.update({self.input_label_ph: batch_label})

                ## === feed all data == ##
                _, loss, l2_loss = self.sess.run([self.train_op, self.loss, self.l2_loss], feed_dict=feed_dict)
                train_loss += loss
            train_loss /= float(nb_train)

            # 计算在开发集上的loss
            dev_loss = self.evaluate(data_dev_dict)

            print('train loss: %f, dev loss: %f, l2 loss: %f' % (train_loss, dev_loss, l2_loss))

            # 根据dev上的表现保存模型
            if not self._path_model:
                continue
            if dev_loss < min_dev_loss:
                min_dev_loss = dev_loss
                current_patience = 0
                # save model
                self.saver.save(self.sess, self._path_model)
                print('model has saved to %s!' % self._path_model)
            else:
                current_patience += 1
                print('no improvement, current patience: %d / %d' %
                      (current_patience, self._train_max_patience))
                if self._train_max_patience and current_patience >= self._train_max_patience:
                    print('\nfinished training! (early stopping, max patience: %d)'
                          % self._train_max_patience)
                    return
        print('\nfinished training!')
        return

    def split_train_dev(self, data_dict, dev_size=0.2):
        """
        划分为开发集和测试集
        Args:
            data_dict: dict, 键: 特征名(or 'label'), 值: np.array
            dev_size: float, 开发集所占的比例，default is 0.2
        Returns:
            data_train_dict, data_dev_dict: same type as data_dict
        """
        data_train_dict, data_dev_dict = dict(), dict()
        for name in data_dict.keys():
            boundary = int((1.-dev_size) * data_dict[name].shape[0])
            data_train_dict[name] = data_dict[name][:boundary]
            data_dev_dict[name] = data_dict[name][boundary:]
        return data_train_dict, data_dev_dict

    def evaluate(self, data_dict):
        """
        计算loss
        Args:
            data_dict: dict
        Return:
            loss: float
        """
        data_count = data_dict['label'].shape[0]
        nb_eval = int(math.ceil(data_count / float(self._batch_size)))
        eval_loss = 0.
        for i in range(nb_eval):
            feed_dict = dict()
            batch_indices = np.arange(i * self._batch_size, (i + 1) * self._batch_size) \
                if (i+1)*self._batch_size <= data_count else \
                np.arange(i * self._batch_size, data_count)
            for feature_name in self._feature_names:  # features and dropout
                batch_data = data_dict[feature_name][batch_indices]
                item = {self.input_feature_ph_dict[feature_name]: batch_data}
                feed_dict.update(item)
                # dropout
                item = {self.weight_dropout_ph_dict[feature_name]: 0.}
                feed_dict.update(item)
            if self._use_char_feature:
                batch_data = data_dict['char'][batch_indices]
                item = {self.input_feature_ph_dict['char']: batch_data}
                feed_dict.update(item)
                item = {self.cnn_dropout_rate_ph: 0.}
                feed_dict.update(item)
            feed_dict.update({self.dropout_rate_ph: 0., self.rnn_dropout_rate_ph: 0.})
            # label feed
            batch_label = data_dict['label'][batch_indices]
            feed_dict.update({self.input_label_ph: batch_label})

            loss = self.sess.run(self.loss, feed_dict=feed_dict)
            eval_loss += loss
        eval_loss /= float(nb_eval)
        return eval_loss

    def predict(self, data_test_dict):
        """
        根据训练好的模型标记数据
        Args:
            data_test_dict: dict
        Return:
            pass
        """
        print('predicting...')
        data_count = data_test_dict[self._feature_names[0]].shape[0]
        nb_test = int(math.ceil(data_count / float(self._batch_size)))
        result_sequences = []  # 标记结果
        for i in tqdm(range(nb_test)):
            feed_dict = dict()
            batch_indices = np.arange(i * self._batch_size, (i + 1) * self._batch_size) \
                if (i+1)*self._batch_size <= data_count else \
                np.arange(i * self._batch_size, data_count)
            for feature_name in self._feature_names:  # features and dropout
                batch_data = data_test_dict[feature_name][batch_indices]
                item = {self.input_feature_ph_dict[feature_name]: batch_data}
                feed_dict.update(item)
                # dropout
                item = {self.weight_dropout_ph_dict[feature_name]: 0.}
                feed_dict.update(item)

            feed_dict.update({self.dropout_rate_ph: 0., self.rnn_dropout_rate_ph: 0.})

            #print 'feed_dict_org', feed_dict
            logits, sequence_actual_length, transition_params = self.sess.run(
                [self.logits, self.sequence_actual_length, self.transition_params], feed_dict=feed_dict)

            #print 'logits', logits,'\n sequence_actual_length',sequence_actual_length,'\n transition_params',transition_params

            for logit, seq_len in zip(logits, sequence_actual_length):
                logit_actual = logit[:seq_len]
                viterbi_sequence, _ = tf.contrib.crf.viterbi_decode(
                    logit_actual, transition_params)
                result_sequences.append(viterbi_sequence)
        #print 'result_sequences',result_sequences
        #print('共标记句子数: %d' % data_count)
        return result_sequences

    def compute_loss(self):
        """
        计算loss

        Return:
            loss: scalar
        """
        if not self._use_crf:
            labels = tf.reshape(
                tf.contrib.layers.one_hot_encoding(
                    tf.reshape(self.input_label_ph, [-1]), num_classes=self._nb_classes),
                shape=[-1, self._sequence_length, self._nb_classes])
            logits = tf.nn.softmax(self.logits, dim=-1)
            cross_entropy = -tf.reduce_sum(labels * tf.log(logits), axis=2)
            mask = tf.sign(tf.reduce_max(tf.abs(labels), axis=2))
            cross_entropy_masked = tf.reduce_sum(
                cross_entropy*mask, axis=1) / tf.cast(self.sequence_actual_length, tf.float32)
            return tf.reduce_mean(cross_entropy_masked)
        else:
            log_likelihood, self.transition_params = tf.contrib.crf.crf_log_likelihood(
                self.logits, self.input_label_ph, self.sequence_actual_length)
            return tf.reduce_mean(-log_likelihood)


import rospy
import numpy as np
import copy
import tf
import datetime
import gc
from tools import *
from pprint import pprint
from pyquaternion import Quaternion
from gpd.msg import GraspConfigList
from moveit_python import *
from moveit_msgs.msg import Grasp, PlaceLocation
from geometry_msgs.msg import PoseStamped, Vector3, Pose
from trajectory_msgs.msg import JointTrajectoryPoint
from visualization_msgs.msg import Marker
from std_msgs.msg import Header, ColorRGBA
from moveit_python.geometry import rotate_pose_msg_by_euler_angles, translate_pose_msg
from tf.transformations import *
import geometry_msgs.msg #for pose2 simple
import math
import sys
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from tf.msg import tfMessage
import time
from send_gripper import open_gripper_summit, close_gripper_summit

from tf import TransformListener
import copy
from control_msgs.msg import *
from trajectory_msgs.msg import *
from sensor_msgs.msg import JointState
import sensor_msgs
import actionlib
from filter_pointcloud_client import call_pointcloud_filter_service
from moveit_commander import MoveGroupCommander, RobotCommander
from moveit_msgs.msg import Constraints, OrientationConstraint, PositionConstraint, JointConstraint
from copy import deepcopy
from pointcloud_operations import create_mesh_and_save
from sensor_msgs import point_cloud2
from std_srvs.srv import Empty

client = None
from moveit_msgs.msg import MoveItErrorCodes

# Build a useful mapping from MoveIt error codes to error names
moveit_error_dict = {}
for name in MoveItErrorCodes.__dict__.keys():
    if not name[:1] == '_':
        code = MoveItErrorCodes.__dict__[name]
        moveit_error_dict[code] = name

class GpdPickPlace(object):
    grasps = []
    grasps_cartesian = []
    mark_pose = False
    grasp_offset = -0.12
    grasp_offset_cartesian = -0.25
    finger_indexes = None
    con_joints_indexes = None
    joint1_con = 0
    joint2_con = 0
    joint3_con = 0
    gripper_closed = False
    global objects_grasped_not_placed
    grasps_received = False

    def __init__(self, mark_pose=False):
        self.grasp_subscriber = rospy.Subscriber("/detect_grasps/clustered_grasps", GraspConfigList, self.grasp_callback)
        if mark_pose:
            self.mark_pose = True
            self.marker_publisher = rospy.Publisher('visualization_marker', Marker, queue_size=10)
        self.p = PickPlaceInterface(group="manipulator", ee_group="endeffector", verbose=True, ns="/summit_xl/")
        self.tf = tf.TransformListener()

    def grasp_callback(self, msg):
        self.grasps = msg.grasps
        self.grasps_received = True
        if (len(msg.grasps)==0):
            pevent("No grasps found, aborting!")
        else:
            pevent("Received new grasps")

    def show_grasp_pose(self, publisher, grasp_pose):
        marker_x = Marker(
            type=Marker.ARROW,
            id=0,
            lifetime=rospy.Duration(60),
            pose=grasp_pose,
            scale=Vector3(0.04, 0.02, 0.02),
            header=Header(frame_id='summit_xl_base_footprint'),
            color=ColorRGBA(1.0, 0.0, 0.0, 0.8))
        publisher.publish(marker_x)

    def get_gpd_grasps(self):
        pevent("Waiting for grasps to arrive")
      #  while len(self.grasps) == 0:
        while (self.grasps_received == False):
            rospy.sleep(0.01)
        return self.grasps

    def generate_grasp_msgs(self, selected_grasps):
            self.grasps = []
            formatted_grasps = []
            self.grasps_cartesian = []
            formatted_grasps_cartesian = []
            tot_grasps = len(selected_grasps)
            cont = 0
            filtered_orientation = 0
            for i in range(0, len(selected_grasps)):
                z_axis_unit = (0, 0, -1)
                ap_axis = (selected_grasps[i].approach.x, selected_grasps[i].approach.y, selected_grasps[i].approach.z)
                angle = numpy.dot(z_axis_unit, ap_axis)
                if (angle <= 0):
                    # filter it out, because grasp coming from below the ground
                    filtered_orientation += 1
                    print(repr(filtered_orientation) + " Grasp filtered because coming from underneath the ground")
                    continue
                g = Grasp()
                g.id = "dupa"
                gp = PoseStamped()
                gp.header.frame_id = "summit_xl_base_footprint"
                org_q = self.trans_matrix_to_quaternion(selected_grasps[i])

                quat = org_q
                gp.pose.position.x = selected_grasps[i].surface.x + self.grasp_offset * selected_grasps[i].approach.x
                gp.pose.position.y = selected_grasps[i].surface.y + self.grasp_offset * selected_grasps[i].approach.y
                gp.pose.position.z = selected_grasps[i].surface.z + self.grasp_offset * selected_grasps[i].approach.z
                gp.pose.orientation.x = float(quat.elements[1])
                gp.pose.orientation.y = float(quat.elements[2])
                gp.pose.orientation.z = float(quat.elements[3])
                gp.pose.orientation.w = float(quat.elements[0])
                g.grasp_pose = gp
                g.pre_grasp_approach.direction.header.frame_id = "arm_ee_link"
                g.pre_grasp_approach.direction.vector.x = 1.0
                g.pre_grasp_approach.min_distance = 0.06
                g.pre_grasp_approach.desired_distance = 0.1
                    #   g.pre_grasp_posture.joint_names = ["gripper_right_finger_joint", "gripper_left_finger_joint"]
                    #   g.pre_grasp_posture.joint_names = ["arm_tool0"]
                    #     g.pre_grasp_posture.header.frame_id = "arm_wrist_3_link"
                    #    pos = JointTrajectoryPoint()
                    #    pos.positions.append(0)
                    #   pos.positions.append(0.1337)
                    #   g.pre_grasp_posture.points.append(pos)
                    #  g.grasp_posture.joint_names = ["gripper_right_finger_joint", "gripper_left_finger_joint"]
                    #  g.grasp_posture.joint_names = ["joint_6"]
                    #  pos = JointTrajectoryPoint()
                    #  pos.positions.append(0.0)
                    #  pos.positions.append(0.0)
                    #  pos.accelerations.append(0.0)
                    #  pos.accelerations.append(0.0)
                    #  g.grasp_posture.points.append(pos)
                    #  g.grasp_posture.header.frame_id = "hand_link"
               # g.allowed_touch_objects = ["<octomap>", "obj"]
                g.allowed_touch_objects = ["obj"]
             #   g.max_contact_force = 0.0
                g.grasp_quality = selected_grasps[0].score.data
                formatted_grasps.append(g)

                #Added code lines for cartesian pick
                gp_cartesian = PoseStamped()
                gp_cartesian.header.frame_id = "summit_xl_base_footprint"
                gp_cartesian.pose.position.x = selected_grasps[i].surface.x + self.grasp_offset_cartesian * selected_grasps[i].approach.x
                gp_cartesian.pose.position.y = selected_grasps[i].surface.y + self.grasp_offset_cartesian * selected_grasps[i].approach.y
                gp_cartesian.pose.position.z = selected_grasps[i].surface.z + self.grasp_offset_cartesian * selected_grasps[i].approach.z
                gp_cartesian.pose.orientation.x = float(quat.elements[1])
                gp_cartesian.pose.orientation.y = float(quat.elements[2])
                gp_cartesian.pose.orientation.z = float(quat.elements[3])
                gp_cartesian.pose.orientation.w = float(quat.elements[0])

                g_cartesian = Grasp ()
                g_cartesian.id = "cart"
                g_cartesian.grasp_pose = gp_cartesian
                g_cartesian.allowed_touch_objects = ["obj"]
                formatted_grasps_cartesian.append(g_cartesian)

           # print(repr(cont) + " grasps out of " + repr(tot_grasps) + " removed because of no IK_SOLUTION error")
            # sort grasps using z (get higher grasps first)
            formatted_grasps.sort(key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True)
            formatted_grasps_cartesian.sort(key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True)
            return formatted_grasps, formatted_grasps_cartesian

    def trans_matrix_to_quaternion(self, grasp):
        r = np.array([[grasp.approach.x, grasp.binormal.x, grasp.axis.x],
                      [grasp.approach.y, grasp.binormal.y, grasp.axis.y],
                      [grasp.approach.z, grasp.binormal.z, grasp.axis.z]])
        return Quaternion(matrix=r)

    def pick(self, grasps_list, verbose=False):
        failed_grasps = 0
        pevent("Pick sequence started")
        # Add object mesh to planning scene
        self.add_object_mesh()
        rospy.sleep(3.0)
        group.set_goal_tolerance(0.01)
        for single_grasp in grasps_list:
            if self.mark_pose:
                self.show_grasp_pose(self.marker_publisher, single_grasp.grasp_pose.pose)
                rospy.sleep(1)
            pevent("Planning grasp:")
            pprint(single_grasp.grasp_pose)
            group.set_start_state_to_current_state()
            group.detach_object("obj")

            ### start code using pick interface ###
            pick_result = group.pick("obj", single_grasp)
            pevent("Planner returned: " + get_moveit_error_code(pick_result))
            if pick_result == 1:
              pevent("Grasp successful!")
              attach_link = "arm_ee_link"
              touch_links = ["gripper_base_link","gripper_left_finger_base_link","gripper_left_finger_link","gripper_right_finger_base_link","gripper_right_finger_link"]
              group.attach_object("obj", attach_link, touch_links)
              group.stop()
              group.clear_pose_targets()
              return single_grasp
            else:
              failed_grasps += 1
              group.stop()
              group.clear_pose_targets()
            ### end code using pick interface ###

            ### start code NOT using pick interface ###
            # group.set_pose_target(single_grasp.grasp_pose.pose)
            # plan = group.plan()
            # if (len(plan.joint_trajectory.points) != 0):
            #    inp = raw_input("Have a look at the planned motion. Do you want to proceed? y/n: ")[0]
            #    if (inp == 'y'):
            #        pevent("Executing grasp: ")
            #        pick_result = group.execute(plan, wait=True)
            #        if pick_result == True:
            #            pevent("Grasp successful!")
            #            attach_link = "arm_ee_link"
            #  	       touch_links = ["gripper_base_link","gripper_left_finger_base_link","gripper_left_finger_link","gripper_right_finger_base_link","gripper_right_finger_link"]
            #            group.attach_object("obj", attach_link, touch_links)
            #            return single_grasp
            #        else:
            #            failed_grasps += 1
            #        group.stop()
            #        group.clear_pose_targets()
            #        group.clear_path_constraints()
            #    elif (inp == 'exit'):
            #        group.stop()
            #        group.clear_pose_targets()
            #        group.clear_path_constraints()
            #        exit(1)
        ### end code NOT using pick interface ###

        self.grasps = []

    def pick_cartesian(self, grasps_list, grasps_list_cartesian, verbose=False):
        failed_grasps = 0
        pevent("Pick sequence started")
        # Add object mesh to planning scene
        self.add_object_mesh()
        rospy.sleep(2.0)
        group.set_goal_tolerance(0.01)
        cont_c = 0
        for single_grasp in grasps_list_cartesian:
            if self.mark_pose:
                self.show_grasp_pose(self.marker_publisher, single_grasp.grasp_pose.pose)
                rospy.sleep(1)
            pevent("Planning grasp:")
            pprint(single_grasp.grasp_pose)
            group.set_start_state_to_current_state()
            group.detach_object("obj")
            group.set_pose_target(single_grasp.grasp_pose.pose)
            plan = group.plan()
            if (len(plan.joint_trajectory.points) != 0):
                inp = raw_input("Have a look at the planned motion. Do you want to proceed? y/n: ")
                if (inp == 'y'):
                    pevent("Executing grasp: ")
                    pick_result = group.execute(plan, wait=True)
                    if pick_result == True:
                        group.stop()
                        group.clear_pose_targets()
                        group.clear_path_constraints()
                        group.set_start_state_to_current_state()
                        if self.mark_pose:
                            self.show_grasp_pose(self.marker_publisher, grasps_list[cont_c].grasp_pose)
                            rospy.sleep(1)
                        group.set_goal_tolerance(0.01)
                        waypoints = []
                        wpose = grasps_list[cont_c].grasp_pose.pose
                        waypoints.append(copy.deepcopy(wpose))
                        # We want the Cartesian path to be interpolated at a resolution of 1 cm
                        # which is why we will specify 0.01 as the eef_step in Cartesian
                        # translation.  We will disable the jump threshold by setting it to 0.0 disabling:
                        (plan, fraction) = group.compute_cartesian_path(
                            waypoints,  # waypoints to follow
                            0.01,  # eef_step
                            0.0)  # jump_threshold
                        waypoints_result = group.execute(plan, wait=True)
                        if waypoints_result == True:
                            pevent("Grasp successful!")
                            attach_link = "arm_ee_link"
                            touch_links = ["gripper_base_link","gripper_left_finger_base_link","gripper_left_finger_link","gripper_right_finger_base_link","gripper_right_finger_link"]
                            group.attach_object("obj", attach_link, touch_links)
                            return single_grasp
                        else:
                            failed_grasps += 1
                            group.stop()
                            group.clear_pose_targets()
                            group.clear_path_constraints()
                    else:
                        failed_grasps += 1
                        group.stop()
                        group.clear_pose_targets()
                        group.clear_path_constraints()
                elif (inp == 'exit'):
                    group.stop()
                    group.clear_pose_targets()
                    group.clear_path_constraints()
                    exit(1)
            cont_c += 1
        self.grasps = []

    def pick_two_steps(self, grasps_list, grasps_list_cartesian, verbose=False):
        failed_grasps = 0
        pevent("Pick sequence started")
        # Add object mesh to planning scene
        self.add_object_mesh()
        rospy.sleep(2.0)
        group.set_goal_tolerance(0.01)
        cont_c = 0
        for single_grasp in grasps_list_cartesian:
            if self.mark_pose:
                self.show_grasp_pose(self.marker_publisher, single_grasp.grasp_pose.pose)
                rospy.sleep(1)
            pevent("Planning grasp:")
            pprint(single_grasp.grasp_pose)
            group.set_start_state_to_current_state()
            group.detach_object("obj")
            group.set_pose_target(single_grasp.grasp_pose.pose)
            plan = group.plan()
            if (len(plan.joint_trajectory.points) != 0):
                inp = raw_input("Have a look at the planned motion. Do you want to proceed? y/n: ")
                if (inp == 'y'):
                    pevent("Executing grasp: ")
                    pick_result = group.execute(plan, wait=True)
                    if pick_result == True:
                        group.stop()
                        group.clear_pose_targets()
                        group.clear_path_constraints()
                        group.set_start_state_to_current_state()
                        if self.mark_pose:
                            self.show_grasp_pose(self.marker_publisher, grasps_list[cont_c].grasp_pose.pose)
                            rospy.sleep(1)
                        group.set_pose_target(grasps_list[cont_c].grasp_pose.pose)
                        plan2 = group.go()
                        if (plan2 == True):
                            pevent("Grasp successful!")
                            attach_link = "arm_ee_link"
                            touch_links = ["gripper_base_link","gripper_left_finger_base_link","gripper_left_finger_link","gripper_right_finger_base_link","gripper_right_finger_link"]
                            group.attach_object("obj", attach_link, touch_links)
                            return single_grasp
                        else:
                            failed_grasps += 1
                            group.stop()
                            group.clear_pose_targets()
                            group.clear_path_constraints()
                    else:
                        failed_grasps += 1
                        group.stop()
                        group.clear_pose_targets()
                        group.clear_path_constraints()
                elif (inp == 'exit'):
                    group.stop()
                    group.clear_pose_targets()
                    group.clear_path_constraints()
                    exit(1)
            cont_c += 1
        self.grasps = []

    def place2(self, place_pose):
        #returns True or False if the place was successfull or not
        pevent("Place sequence started")
        pose_goal = geometry_msgs.msg.Pose()
        pose_goal.position.x = 0.516344249249
        pose_goal.position.y = -0.636391639709
        pose_goal.position.z =   0.603573918343
        pose_goal.orientation.w = 0.185884654522
        pose_goal.orientation.x = -0.681892871857
        pose_goal.orientation.y = 0.682668983936
        pose_goal.orientation.z = 0.185558646917
        group.set_start_state_to_current_state()
        group.set_pose_target(pose_goal)
        pevent("Planning pose:")
        pprint(pose_goal)
        group.set_pose_target(pose_goal)
        plan = group.plan()
        rospy.sleep(1)
        cont_plan_place=0
        place_successful=False
        while ((len(plan.joint_trajectory.points) == 0) and (cont_plan_place <10)):
            plan = group.plan()
            rospy.sleep(1)
            cont_plan_place+=1
        if (len(plan.joint_trajectory.points) != 0):
            inp = raw_input("Have a look at the planned motion. Do you want to proceed? y/n: ")[0]
            if (inp == 'y'):
                pevent("Executing place: ")
                pick_result = group.execute(plan, wait=True)
                if pick_result == True:
                    pevent("Pose successful!")
                    group.detach_object("obj")
                    place_successful=True
                else:
                    pevent("Pose failed!")
                # Calling `stop()` ensures that there is no residual movement
                group.stop()
                group.clear_pose_targets()
                place_successful = False
            elif (inp == 'exit'):
                group.stop()
                group.clear_pose_targets()
                place_successful = False
                exit(1)
        else:
            place_successful=False
            group.stop()
            group.clear_pose_targets()
        return place_successful

    def place(self, place_pose):
        pevent("Place sequence started")
        places = self.generate_place_poses(place_pose)
        place_result = self.p.place_with_retry("obj", places, support_name="<octomap>", planning_time=9001,
                                  goal_is_eef=True)
               # pevent("Planner returned: " + get_moveit_error_code(place_result.error_code.val))

    def generate_place_poses(self, initial_place_pose):
        places = list()
        l = PlaceLocation()
        l.id = "dupadupa"
        l.place_pose.header.frame_id = "summit_xl_base_footprint"
        q = Quaternion(initial_place_pose.grasp_pose.pose.orientation.w,
                        initial_place_pose.grasp_pose.pose.orientation.x,
                        initial_place_pose.grasp_pose.pose.orientation.y,
                        initial_place_pose.grasp_pose.pose.orientation.z)
# Load successful grasp pose
        l.place_pose.pose.position = initial_place_pose.grasp_pose.pose.position
        l.place_pose.pose.orientation.w = q.elements[0]
        l.place_pose.pose.orientation.x = q.elements[1]
        l.place_pose.pose.orientation.y = q.elements[2]
        l.place_pose.pose.orientation.z = q.elements[3]
# Move 20cm to the right
        l.place_pose.pose.position.y += 0.2
# Fill rest of the msg with some data
        l.post_place_posture = initial_place_pose.grasp_posture
        l.post_place_retreat = initial_place_pose.post_grasp_retreat
        l.pre_place_approach = initial_place_pose.pre_grasp_approach
        places.append(copy.deepcopy(l))
# Rotate place pose to generate more possible configurations for the planner
        m = 16  # Number of possible place poses
        for i in range(0, m - 1):
            l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * math.pi / m)
            places.append(copy.deepcopy(l))
        return places

    def add_object_mesh(self):
        obj_pose = PoseStamped()
        obj_pose.header.frame_id = "summit_xl_base_footprint"
        obj_pose.pose.position.x = 0
        obj_pose.pose.position.y = 0
        obj_pose.pose.position.z = 0
        obj_pose.pose.orientation.x = 0
        obj_pose.pose.orientation.y = 0
        obj_pose.pose.orientation.z = 0
        obj_pose.pose.orientation.w = 1
        #remove collision object from previous run
        planning.removeCollisionObject("obj")
        rospy.sleep(1)
        planning.addMesh("obj", obj_pose.pose, "object.stl")
        print("Collision object is:")
        rospy.sleep(1)
        pprint(planning.getKnownCollisionObjects())

    def set_pose_constraints(self,tol_joint1,tol_joint2,tol_joint3):
        angle_constraints = Constraints()
        joint_constraint = JointConstraint()
        angle_constraints.name = "angle"
        joint_constraint.position = self.joint1_con
        joint_constraint.tolerance_above = tol_joint1
        joint_constraint.tolerance_below = tol_joint1
        joint_constraint.weight = 0.1
        joint_constraint.joint_name = "arm_shoulder_pan_joint"
        angle_constraints.joint_constraints.append(joint_constraint)

        joint_constraint2 = JointConstraint()
        joint_constraint2.position = self.joint2_con
        joint_constraint2.tolerance_above = tol_joint2
        joint_constraint2.tolerance_below = tol_joint2
        joint_constraint2.weight = 0.2
        joint_constraint2.joint_name = "arm_wrist_2_joint"
        angle_constraints.joint_constraints.append(joint_constraint2)

        joint_constraint3 = JointConstraint()
        joint_constraint3.position = self.joint3_con
        joint_constraint3.tolerance_above = tol_joint3
        joint_constraint3.tolerance_below = tol_joint3
        joint_constraint3.weight = 0.3
        joint_constraint3.joint_name = "arm_wrist_1_joint"
        angle_constraints.joint_constraints.append(joint_constraint3)
        group.set_path_constraints(angle_constraints)

    def set_upright_constraints(self,pose):
        upright_constraints = Constraints()
        orientation_constraint = OrientationConstraint()
        upright_constraints.name = "upright"
        orientation_constraint.header = pose.header
        orientation_constraint.link_name = group.get_end_effector_link()
        orientation_constraint.orientation = pose.pose.orientation
        orientation_constraint.absolute_x_axis_tolerance = 3.14
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 0.1
        #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
        orientation_constraint.weight = 1
        upright_constraints.orientation_constraints.append(orientation_constraint)
        group.set_path_constraints(upright_constraints)

    def start_con_setup(self):
        self.joints_subscriber = rospy.Subscriber('/summit_xl/joint_states', JointState, self.joints_callback)

    def stop_con_setup(self):
        self.joints_subscriber.unregister()

    def joints_callback(self, data):
        joint_names=['arm_shoulder_pan_joint','arm_wrist_1_joint','arm_wrist_2_joint']
        names = data.name
        if joint_names[0] in (names):
            j1_index = names.index("arm_shoulder_pan_joint")
            if joint_names[1] in (names):
                j2_index = names.index("arm_wrist_1_joint")
                if joint_names[2] in (names):
                    j3_index = names.index("arm_wrist_2_joint")
                    self.con_joints_indexes = (j1_index, j2_index, j3_index)
                    self.joint1_con = data.position[self.con_joints_indexes[0]]
                    self.joint2_con = data.position[self.con_joints_indexes[1]]
                    self.joint3_con = data.position[self.con_joints_indexes[2]]

    def remove_pose_constraints(self):
        group.clear_path_constraints()

    def drop_obj_on_robot(self, successful_grasp):
        pevent("Dropping object on robot")
        pose_goal = geometry_msgs.msg.Pose()
        pose_goal.position.x = -0.2
        pose_goal.position.y = 0
        pose_goal.position.z = 0.4+0.08+successful_grasp.grasp_pose.pose.position.z
        pose_goal.orientation.x = 0+successful_grasp.grasp_pose.pose.orientation.x
        pose_goal.orientation.y = 0+successful_grasp.grasp_pose.pose.orientation.y
        pose_goal.orientation.z = 0+successful_grasp.grasp_pose.pose.orientation.z
        pose_goal.orientation.w = successful_grasp.grasp_pose.pose.orientation.w
        group.set_start_state_to_current_state()
        group.set_goal_tolerance(0.05)
        group.set_pose_target(pose_goal)

        # The go command can be called with joint values, poses, or without any
        # parameters if you have already set the pose or joint target for the group
        #success = group.go(wait=True)
        # if (success==True):
        #     open_gripper_summit()
        #     print("Gripper opened")
        #     group.detach_object("obj")
        # group.clear_pose_targets()
        # group.stop()
        # return success

        plan = group.plan()
        rospy.sleep(1)
        cont_plan_drop = 0
        while ((len(plan.joint_trajectory.points) == 0) and (cont_plan_drop < 10)):
            plan = group.plan()
            rospy.sleep(1)
            cont_plan_drop += 1
        if (len(plan.joint_trajectory.points) != 0):
            inp = raw_input("Have a look at the planned motion. Do you want to proceed? y/n: ")[0]
            if (inp == 'y'):
                pevent("Executing dropping: ")
                result = group.execute(plan, wait=True)
                rospy.sleep(1)
                if result == True:
                    pevent("Dropping successful!")
                    group.detach_object("obj")
                    group.stop()
                    group.clear_pose_targets()
                    open_gripper_summit()
                    print("Gripper opened")
                    return True
                else:
                    pevent("Dropping failed!")
                    group.stop()
                    group.clear_pose_targets()
                    return False
            elif (inp == 'exit'):
                group.stop()
                group.clear_pose_targets()
                group.clear_path_constraints()
                exit(1)
        else:
            pevent("Dropping position planning failed. Aborting")
            return False

    def initial_pose(self):
        pevent("Initial constrained pose sequence started")
        pose_goal = geometry_msgs.msg.Pose()
        pose_goal.position.x = 1.06285917759
        pose_goal.position.y = -0.00109632476233
        pose_goal.position.z = 0.828287780285
        pose_goal.orientation.w = 0.512402296066
        pose_goal.orientation.x = -0.512062907219
        pose_goal.orientation.y = 0.470357865095
        pose_goal.orientation.z = 0.503983080387
        group.set_start_state_to_current_state()
        group.set_goal_tolerance(0.05)
        group.set_pose_target(pose_goal)

        # The go command can be called with joint values, poses, or without any
        # parameters if you have already set the pose or joint target for the group
        # group.go(joint_goal, wait=True)
        plan = group.plan()
        rospy.sleep(1)
        cont_plan = 0
        while ((len(plan.joint_trajectory.points) == 0) and (cont_plan < 3)):
            plan = group.plan()
           # rospy.sleep(1)
            cont_plan += 1
        group.clear_path_constraints()
        if (len(plan.joint_trajectory.points) != 0):
            inp = raw_input("Have a look at the planned motion. Do you want to proceed? y/n: ")[0]
            if (inp == 'y'):
                pevent("Executing place: ")
                result = group.execute(plan, wait=True)
                rospy.sleep(1)
                if result == True:
                    pevent("Initial pose successful!")
                    group.stop()
                    group.clear_pose_targets()
                    return True
                else:
                    pevent("Initial pose failed!")
                    group.stop()
                    group.clear_pose_targets()
                    return False
            elif (inp == 'exit'):
                group.stop()
                group.clear_pose_targets()
                group.clear_path_constraints()
                exit(1)
        else:
            pevent("Initial position planning failed. Aborting")
            return False

    def wait_for_mesh_and_save(self):
      pinfo("Subscribing to pointcloud to generate mesh")
      self.obj_pc_subscriber = rospy.Subscriber("/cloud_indexed_pc_only", sensor_msgs.msg.PointCloud2 , self.obj_pointcloud_callback)

    def obj_pointcloud_callback(self, msg):
      pinfo("Pointcloud received")
      cloud = []
      for p in point_cloud2.read_points(msg, skip_nans=True):
                cloud.append([p[0], p[1], p[2]])
      create_mesh_and_save(cloud)
      pinfo("Mesh generated")
      self.obj_pc_subscriber.unregister()

    def gripper_callback(self, data):
        if (self.finger_indexes == None):
            names = data.name
            lf_index = names.index("gripper_left_finger_base_joint")
            rf_index = names.index("gripper_right_finger_base_joint")
            self.finger_indexes = (lf_index, rf_index)

        lf_joint = data.position[self.finger_indexes[0]]
        rf_joint = data.position[self.finger_indexes[1]]

        closed_range = 0.003

        if (lf_joint < closed_range and rf_joint < closed_range):
            if (not self.gripper_closed):
                pprint("Gripper closed, we probably lost the grip")
            self.gripper_closed = True

    def start_grasp_check(self):
        # subscribe to topic to derive gripper position
        self.fingers_subscriber = rospy.Subscriber('/summit_xl/joint_states', JointState, self.gripper_callback)

    def stop_grasp_check(self):
        self.fingers_subscriber.unregister()
        return self.gripper_closed

if __name__ == "__main__":
    start_time = datetime.datetime.now()
    rospy.init_node("gpd_pick_and_place",anonymous=True)
    close_gripper_summit()
    tf_listener_ = TransformListener()
    pnp = GpdPickPlace(mark_pose=True)
    group_name = "manipulator"
    group = moveit_commander.MoveGroupCommander(group_name, robot_description="/summit_xl/robot_description", ns="/summit_xl")
   # group.set_planner_id("BiTRRT")
  #  group.set_max_velocity_scaling_factor(0.05)
   # group.set_goal_orientation_tolerance(0.01)
    group.set_planning_time(3)
   # group.allow_replanning(True)
    planning = PlanningSceneInterface("summit_xl_base_footprint", ns="/summit_xl/")
    planning.clear()
    rospy.sleep(1)
    num_objects = 2
    succesfull_objects_placements = 0
    objects_grasped_lost = 0
    objects_grasped_not_placed = 0
    rospy.wait_for_service('/summit_xl/clear_octomap')
    clear_octomap = rospy.ServiceProxy('/summit_xl/clear_octomap', Empty)
    clear_octomap()

    for r in range (0, num_objects):
        planning.clear()
        # Subscribe for grasps
        print("--- Move Arm to Initial Position---")
      #  print "Please make sure that your robot can move freely in vertical before proceeding!"
      #  inp = raw_input("Do you want to proceed? y/n: ")[0]
      #  if (inp == 'y'):
        pnp.remove_pose_constraints()
        pnp.start_con_setup()
        rospy.sleep(1)
        pnp.set_pose_constraints(1.57, 3.14, 3.14)
        pnp.stop_con_setup()
        while (pnp.initial_pose() == False):
            print("Initial arm positioning failed!")
        print("Initial arm positioning performed")
        clear_octomap()
        #we have to add a check, so that this is called only if the initial_pose was successful
        call_pointcloud_filter_service()
        pnp.wait_for_mesh_and_save()
    # Wait for grasps from gpd, wrap them into Grasp msg format and start picking
        pnp.grasps_received = False
        selected_grasps = pnp.get_gpd_grasps()
        [formatted_grasps, formatted_grasps_cartesian] = pnp.generate_grasp_msgs(selected_grasps)
        open_gripper_summit()
        print("Gripper opened")
        pnp.remove_pose_constraints()
       # pnp.start_con_setup()
       # pnp.set_pose_constraints(1.57, 1.57, 1.57)
       # pnp.stop_con_setup()
        successful_grasp = pnp.pick(formatted_grasps, verbose=True)
      #  successful_grasp = pnp.pick_cartesian(formatted_grasps, formatted_grasps_cartesian, verbose=True)
      #  successful_grasp = pnp.pick_two_steps(formatted_grasps, formatted_grasps_cartesian, verbose=True)
        if successful_grasp is not None:
            close_gripper_summit()
            print("Gripper closed")
            pnp.remove_pose_constraints()
         #   pnp.start_con_setup()
         #   rospy.sleep(1)
         #   pnp.set_pose_constraints(3.14, 1.0, 1.0)
         #   pnp.stop_con_setup()
          #  pnp.set_upright_constraints(successful_grasp.grasp_pose)
            while (pnp.drop_obj_on_robot(successful_grasp) == False):
                print("Object placing failed!")
          #  if success == False:
          #      objects_grasped_not_placed += 1
          #  else:
            succesfull_objects_placements += 1
        else:
            print("Grasp NOT performed")
            pnp.remove_pose_constraints()

        perc_successful_grasps = (100.0 * succesfull_objects_placements/num_objects)

    pnp.remove_pose_constraints()
    print(str(succesfull_objects_placements) + " out of " + str(num_objects) + " succesfull grasps, that is: %.2f" % perc_successful_grasps  + " and of the grasped once the not placed ones are " + str(objects_grasped_not_placed))
    pinfo("Demo runtime: " + str(datetime.datetime.now() - start_time))


# -*- coding:utf-8 -*-

from __future__ import absolute_import
from __future__ import print_function
from __future__ import division

import os
import sys
import tensorflow as tf
import cv2
import numpy as np
import math
from tqdm import tqdm
import argparse
from multiprocessing import Queue, Process
sys.path.append("../")

from data.io.image_preprocess import short_side_resize_for_inference_data
from libs.networks import build_whole_network
from help_utils import tools
from libs.label_name_dict.label_dict import *
from libs.box_utils import draw_box_in_img
from libs.box_utils.coordinate_convert import forward_convert, backward_convert

def worker(gpu_id, images, det_net, result_queue):
    os.environ["CUDA_VISIBLE_DEVICES"] = str(gpu_id)
    # 1. preprocess img
    img_plac = tf.placeholder(dtype=tf.uint8, shape=[None, None, 3])  # is RGB. not BGR
    img_batch = tf.cast(img_plac, tf.float32)

    img_batch = short_side_resize_for_inference_data(img_tensor=img_batch,
                                                     target_shortside_len=cfgs.IMG_SHORT_SIDE_LEN,
                                                     length_limitation=cfgs.IMG_MAX_LENGTH)
    if cfgs.NET_NAME in ['resnet152_v1d', 'resnet101_v1d', 'resnet50_v1d']:
        img_batch = (img_batch / 255 - tf.constant(cfgs.PIXEL_MEAN_)) / tf.constant(cfgs.PIXEL_STD)
    else:
        img_batch = img_batch - tf.constant(cfgs.PIXEL_MEAN)

    img_batch = tf.expand_dims(img_batch, axis=0)

    detection_boxes, detection_scores, detection_category = det_net.build_whole_detection_network(
        input_img_batch=img_batch,
        gtboxes_batch_h=None,
        gtboxes_batch_r=None)

    init_op = tf.group(
        tf.global_variables_initializer(),
        tf.local_variables_initializer()
    )

    restorer, restore_ckpt = det_net.get_restorer()

    config = tf.ConfigProto()
    config.gpu_options.allow_growth = True

    with tf.Session(config=config) as sess:
        sess.run(init_op)
        if not restorer is None:
            restorer.restore(sess, restore_ckpt)
            print('restore model %d ...' % gpu_id)
        for a_img in images:
            raw_img = cv2.imread(a_img)
            raw_h, raw_w = raw_img.shape[0], raw_img.shape[1]

            resized_img, detected_boxes, detected_scores, detected_categories = \
                sess.run(
                    [img_batch, detection_boxes, detection_scores, detection_category],
                    feed_dict={img_plac: raw_img[:, :, ::-1]}
                )
            detected_boxes = forward_convert(detected_boxes, False)

            detected_indices = detected_scores >= cfgs.VIS_SCORE
            detected_scores = detected_scores[detected_indices]
            detected_boxes = detected_boxes[detected_indices]
            detected_categories = detected_categories[detected_indices]

            resized_h, resized_w = resized_img.shape[1], resized_img.shape[2]
            scales = [raw_w / resized_w, raw_h / resized_h]
            result_dict = {'scales': scales, 'boxes': detected_boxes,
                           'scores': detected_scores, 'labels': detected_categories,
                           'image_id': a_img}
            result_queue.put_nowait(result_dict)

def test_icdar2015(det_net, real_test_img_list, gpu_ids, show_box):

    save_path = os.path.join('./test_icdar2015', cfgs.VERSION)
    tools.mkdir(save_path)

    nr_records = len(real_test_img_list)
    pbar = tqdm(total=nr_records)
    gpu_num = len(gpu_ids.strip().split(','))

    nr_image = math.ceil(nr_records / gpu_num)
    result_queue = Queue(500)
    procs = []

    for i in range(gpu_num):
        start = i * nr_image
        end = min(start + nr_image, nr_records)
        split_records = real_test_img_list[start:end]
        proc = Process(target=worker, args=(i, split_records, det_net, result_queue))
        print('process:%d, start:%d, end:%d' % (i, start, end))
        proc.start()
        procs.append(proc)

    for i in range(nr_records):
        res = result_queue.get()

        x1, y1, x2, y2, x3, y3, x4, y4 = res['boxes'][:, 0], res['boxes'][:, 1], res['boxes'][:, 2], res['boxes'][:, 3],\
                                         res['boxes'][:, 4], res['boxes'][:, 5], res['boxes'][:, 6], res['boxes'][:, 7]

        x1, y1 = x1 * res['scales'][0], y1 * res['scales'][1]
        x2, y2 = x2 * res['scales'][0], y2 * res['scales'][1]
        x3, y3 = x3 * res['scales'][0], y3 * res['scales'][1]
        x4, y4 = x4 * res['scales'][0], y4 * res['scales'][1]

        boxes = np.transpose(np.stack([x1, y1, x2, y2, x3, y3, x4, y4]))

        if show_box:
            boxes = backward_convert(boxes, False)
            nake_name = res['image_id'].split('/')[-1]
            draw_path = os.path.join(save_path, nake_name)
            draw_img = np.array(cv2.imread(res['image_id']), np.float32)

            final_detections = draw_box_in_img.draw_boxes_with_label_and_scores(draw_img,
                                                                                boxes=boxes,
                                                                                labels=res['labels'],
                                                                                scores=res['scores'],
                                                                                method=1,
                                                                                in_graph=False)
            cv2.imwrite(draw_path, final_detections)

        else:
            fw_txt_dt = open(os.path.join(save_path, 'res_{}.txt'.format(res['image_id'].split('/')[-1].split('.')[0])), 'w')

            for box in boxes:
                line = '%d,%d,%d,%d,%d,%d,%d,%d\n' % (box[0], box[1], box[2], box[3],
                                                      box[4], box[5], box[6], box[7])
                fw_txt_dt.write(line)
            fw_txt_dt.close()

        pbar.set_description("Test image %s" % res['image_id'].split('/')[-1])

        pbar.update(1)

    for p in procs:
        p.join()

def eval(num_imgs, test_dir, gpu_ids, show_box):

    test_imgname_list = [os.path.join(test_dir, img_name) for img_name in os.listdir(test_dir)
                         if img_name.endswith(('.jpg', '.png', '.jpeg', '.tif', '.tiff'))]
    assert len(test_imgname_list) != 0, 'test_dir has no imgs there.' \
                                        ' Note that, we only support img format of (.jpg, .png, and .tiff) '

    if num_imgs == np.inf:
        real_test_img_list = test_imgname_list
    else:
        real_test_img_list = test_imgname_list[: num_imgs]

    retinanet = build_whole_network.DetectionNetwork(base_network_name=cfgs.NET_NAME,
                                                     is_training=False)
    test_icdar2015(det_net=retinanet, real_test_img_list=real_test_img_list, gpu_ids=gpu_ids, show_box=show_box)

def parse_args():

    parser = argparse.ArgumentParser('evaluate the result with Pascal2007 stdand')

    parser.add_argument('--test_dir', dest='test_dir',
                        help='evaluate imgs dir ',
                        default='/data/ICDAR2015/test', type=str)
    parser.add_argument('--gpus', dest='gpus',
                        help='gpu id',
                        default='0,1,2,3,4,5,6,7', type=str)
    parser.add_argument('--eval_num', dest='eval_num',
                        help='the num of eval imgs',
                        default=np.inf, type=int)
    parser.add_argument('--show_box', '-s', default=False,
                        action='store_true')
    args = parser.parse_args()
    return args

if __name__ == '__main__':

    args = parse_args()
    print(20*"--")
    print(args)
    print(20*"--")
    eval(args.eval_num,
         test_dir=args.test_dir,
         gpu_ids=args.gpus,
         show_box=args.show_box)


import cv2
import sys
import numpy as np
from PIL import Image

# 사진과 얼굴의 어떤 부분을 인식할까요?
imagePath = sys.argv[1]
cascPath = "haarcascade_frontalface_default.xml"
print(cv2.CascadeClassifier(cascPath))
# haar cascade를 결정합니다
faceCascade = cv2.CascadeClassifier(cascPath)

# 이미지를 읽어와 흑백사진으로 전환합니다
image = cv2.imread(imagePath)
i = Image.open(imagePath)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

# 흑백사진 상태에서 얼굴을 탐지합니다
faces = faceCascade.detectMultiScale(
    gray,
    scaleFactor=1.1,
    minNeighbors=10,
    minSize=(30, 30)
    # flags = cv2.cv.CV_HAAR_SCALE_IMAGE
)

# 몇개의 얼굴을 찾았는지 말합니다
# 얼굴이 없는 사람들이 종종 있어요
print("Found {0} faces!".format(len(faces)))
if len(faces) > 1:
    print("얼굴2개이상감지. 추후 서버작업필")
elif len(faces) == 0:
    print("얼굴 안나옴. 추후 서버작업필")

width, height  = i.size
print(width, height)
f=open('size.txt','w')
f.write(str(width));
f.write(',');
f.write(str(height));
f.close()
# 얼굴에 초록색 사각형 그려주기 -> RGB (0, 255, 0)
# 빨강은 (255,0,0), 파랑은 (0,0,255)
for (x, y, w, h) in faces:
    cv2.rectangle(image, (x, y), (x+w, y+h), (0, 255, 0), 2)

print(faces);
np.savetxt('xy.txt',faces,fmt='%d',delimiter=",")
 #   cropped = image[y - int(h/1.9):y + h + int(h/1.5), x - int(w/4):x + w + int(w/4)]
   # cropped = image[y - int(h/1.66):y + h + int(h*1.23), x - int(w/4):x + w + int(w/4)]
    # 원으로 그린다면?
    # cv2.circle(image,(x+int(w/2),y+int(h/2)), w, (0,0,255), 2)

# 얼굴에 사각형을 친 사진을 출력합니다
cv2.imwrite("Faces.png", image)
# cv2.imwrite("Faces_circle.png", image)
cv2.waitKey(0)


import sys
sys.path.append(".")
import yaml, os, json
from triplet_reid.edflow_implementations.deepfashion.data import FromCSVWithEmbedding
from tqdm import trange
import numpy as np

from triplet_reid.excluders.diagonal import Excluder as DiagonalExcluder
from scipy.spatial.distance import cdist
from sklearn.metrics import average_precision_score

def evaluate(query_dataset, gallery_dataset, embedding_root, embedding_postfix,
        n_retrievals = 10, cutoff = 2000):
    # load required data
    def load(data):
        keys = ["pid", "name", "embedding"]
        loaded = dict((k, list()) for k in keys)
        for i in trange(len(data)):
            datum = data[i]
            for k in keys:
                loaded[k].append(datum[k])
        for k in keys:
            loaded[k] = np.stack(loaded[k])
        return loaded

    query_data = load(query_dataset)
    gallery_data = load(gallery_dataset)

    # evaluation as in original evaluate.py
    distances = cdist(query_data["embedding"], gallery_data["embedding"],
            metric = "euclidean")

    # Compute the pid matches
    pid_matches = gallery_data["pid"][None] == query_data["pid"][:,None]

    # Get a mask indicating True for those gallery entries that should
    # be ignored for whatever reason (same camera, junk, ...) and
    # exclude those in a way that doesn't affect CMC and mAP.
    Excluder = DiagonalExcluder
    excluder = Excluder(gallery_data["name"])
    mask = excluder(query_data["name"])
    distances[mask] = np.inf
    pid_matches[mask] = False

    # Keep track of statistics. Invert distances to scores using any
    # arbitrary inversion, as long as it's monotonic and well-behaved,
    # it won't change anything.
    aps = []
    cmc = np.zeros(len(gallery_data["pid"]), dtype=np.int32)
    scores = 1 / (1 + distances)
    for i in range(len(distances)):
        ap = average_precision_score(pid_matches[i], scores[i])

        if np.isnan(ap):
            logwarn = print
            logwarn("WARNING: encountered an AP of NaN!")
            logwarn("This usually means a person only appears once.")
            logwarn("In this case, it's because of {}.".format(query_data["name"][i]))
            logwarn("I'm excluding this person from eval and carrying on.")
            continue

        aps.append(ap)
        # Find the first true match and increment the cmc data from there on.
        k = np.where(pid_matches[i, np.argsort(distances[i])])[0][0]
        cmc[k:] += 1

    # Compute the actual cmc and mAP values
    cmc = cmc / len(query_data["pid"])
    mean_ap = np.mean(aps)

    # Print out a short summary.
    info = '{}/**/*{} | mAP: {:.2%} | top-1: {:.2%} top-2: {:.2%} | top-5: {:.2%} | top-10: {:.2%}'.format(
        embedding_root, embedding_postfix, mean_ap, cmc[0], cmc[1], cmc[4], cmc[9])
    print(info)

    # Retrieval data for easy plotting
    retrievals = np.argsort(distances, axis = 1)
    print(distances.shape)
    print(retrievals.shape)
    retrievals = np.concatenate(
            [retrievals[:, :n_retrievals], retrievals[:, -n_retrievals-cutoff:-cutoff]], axis = 1)
    query_names = query_data["name"]
    retrieval_names = [
            [gallery_data["name"][retrieve_idx] for retrieve_idx in retrievals[query_idx]]
            for query_idx in range(retrievals.shape[0])]
    retrieval_match = [
            [pid_matches[query_idx, retrieve_idx] for retrieve_idx in retrievals[query_idx]]
            for query_idx in range(retrievals.shape[0])]
    retrieval_data = {
            "query_names": query_names,
            "retrieval_names": retrieval_names,
            "retrieval_match": retrieval_match}
    retrieval_data["info"] = {
            "mAP": mean_ap,
            "cmc": cmc}

    return info, retrieval_data

class NumpyEncoder(json.JSONEncoder):
    """ Special json encoder for numpy types """
    def default(self, obj):
        if isinstance(obj, (np.int_, np.intc, np.intp, np.int8,
            np.int16, np.int32, np.int64, np.uint8,
            np.uint16, np.uint32, np.uint64)):
            return int(obj)
        elif isinstance(obj, (np.float_, np.float16, np.float32,
            np.float64)):
            return float(obj)
        elif isinstance(obj, (np.bool_,)):
            return bool(obj)
        elif isinstance(obj,(np.ndarray,)): #### This is the fix
            return obj.tolist()
        return json.JSONEncoder.default(self, obj)

def run(embedding_root, embedding_postfix, z_size, n_retrievals = 10, cutoff = 2000):
    query_config = {
            "spatial_size":         256,
            "data_root":            "data/deepfashion/images",
            "embedding_root":       embedding_root,
            "embedding_postfix":    embedding_postfix,
            "data_csv":             "data/deepfashion/query.csv",
            "z_size":               z_size}
    query_dataset = FromCSVWithEmbedding(query_config)
    gallery_config = {
            "spatial_size":         256,
            "data_root":            "data/deepfashion/images",
            "embedding_root":       embedding_root,
            "embedding_postfix":    embedding_postfix,
            "data_csv":             "data/deepfashion/gallery.csv",
            "z_size":               z_size}
    gallery_dataset = FromCSVWithEmbedding(gallery_config)
    print(len(query_dataset))
    print(len(gallery_dataset))
    info, retrieval_data = evaluate(query_dataset, gallery_dataset,
            embedding_root, embedding_postfix, n_retrievals, cutoff)
    out_path = "retrieval_data" + embedding_postfix + ".json"
    out_path = os.path.join(embedding_root, out_path)
    with open(out_path, "w") as f:
        json.dump(retrieval_data, f, cls = NumpyEncoder)
    print("Wrote {}".format(out_path))

def old():
    #embedding_root = "data/deepfashion/embeddings"
    #embedding_postfix = "_alpha.npz"

    #embedding_root = "data/deepfashion/embeddings_both_df128_450000"
    embedding_root = "data/deepfashion/embeddings_first_stage_450000"
    #embedding_postfix = "_z_cond.npy"
    embedding_postfix = "_z_posterior_parameters.npy"

    z_size = None
    if embedding_postfix == "_z_posterior_parameters.npy":
        z_size = 128

    run(embedding_root, embedding_postfix, z_size)

if __name__ == "__main__":
    import argparse
    parser = argparse.ArgumentParser()
    parser.add_argument("embedding_root")
    parser.add_argument("embedding_postfix")
    parser.add_argument("--z_size", default = None)
    parser.add_argument("--n_retrievals", default = 10, type = int)
    parser.add_argument("--cutoff", default = 2000, type = int)
    opt = parser.parse_args()
    run(opt.embedding_root, opt.embedding_postfix, opt.z_size, opt.n_retrievals, opt.cutoff)


"""Implementation on top of MultiNest via PyMultiNest.

This module defines the class for Nested Sampling using the MultiNest
program via its Python wrapper PyMultiNest. Note that PyMultiNest and MultiNest
have to be built and installed separately (from gleipnir) before this module
can be used.

PyMultiNest: https://github.com/JohannesBuchner/PyMultiNest
MultiNest: https://github.com/JohannesBuchner/MultiNest

References:
    MultiNest:
    1. Feroz, Farhan, and M. P. Hobson. "Multimodal nested sampling: an
        efficient and robust alternative to Markov Chain Monte Carlo
        methods for astronomical data analyses." Monthly Notices of the
        Royal Astronomical Society 384.2 (2008): 449-463.
    2. Feroz, F., M. P. Hobson, and M. Bridges. "MultiNest: an efficient
        and robust Bayesian inference tool for cosmology and particle
        physics." Monthly Notices of the Royal Astronomical Society 398.4
        (2009): 1601-1614.
    3. Feroz, F., et al. "Importance nested sampling and the MultiNest
        algorithm." arXiv preprint arXiv:1306.2144 (2013).
    PyMultiNest:
    4. Buchner, J., et al. "X-ray spectral modelling of the AGN obscuring
        region in the CDFS: Bayesian model selection and catalogue."
        Astronomy & Astrophysics 564 (2014): A125.

"""

import numpy as np
import warnings
from .nsbase import NestedSamplingBase

try:
    import pymultinest
    from pymultinest.solve import solve
    from pymultinest.analyse import Analyzer
except ImportError as err:
    #print(err)
    raise err

class MultiNestNestedSampling(NestedSamplingBase):
    """Nested Sampling using MultiNest.
    PyMultiNest: https://github.com/JohannesBuchner/PyMultiNest
    MultiNest: https://github.com/JohannesBuchner/MultiNest

    Attributes:
        sampled_parameters (list of :obj:gleipnir.sampled_parameter.SampledParameter):
            The parameters that are being sampled during the Nested Sampling
            run.
        loglikelihood (function): The log-likelihood function to use for
            assigning a likelihood to parameter vectors during the sampling.
        population_size (int): The number of points to use in the Nested
            Sampling active population.
        multinest_kwargs (dict): Additional keyword arguments that should be
            passed to the PyMultiNest MultiNest solver. Available options are:
                importance_nested_sampling (bool): Should MultiNest use
                    Importance Nested Sampling (INS). Default: True
                constant_efficiency_mode (bool): Should MultiNest run in
                    constant sampling efficiency mode. Default: False
	            sampling_efficiency (float): Set the MultiNest sampling
                    efficiency. 0.3 is recommended for evidence evaluation,
                    while 0.8 is recommended for parameter estimation.
                    Default: 0.8
                resume (bool): Resume from a previous MultiNest run (using
                    the last saved checkpoint in the MultiNest output files).
                    Default: True
                write_output (bool): Specify whether MultiNest should write
                    to output files. True is required for additional
                    analysis. Default: True
                multimodal (bool): Set whether MultiNest performs mode
                    separation. Default: True
                max_mode (int): Set the maximum number of modes allowed in
                    mode separation (if multimodal=True). Default: 100
                mode_tolerance (float): A lower bound for which MultiNest will
                    use to separate mode samples and statistics with
                    log-evidence value greater the given value.
                    Default: -1e90
                n_clustering_params (int): If multimodal=True, set the number
                    of parameters to use in clustering during mode separation.
                    If None, then MultiNest will use all the paramters for
                    clustering during mode separation. If
                    n<(number of sampled parameters), then MultiNest will only
                    use a subset composed of the first n parameters for
                    clustering during mode separation. Default: None
                null_log_evidence (float): If multimodal=True, a lower bound
                    for which MultiNest can use to separte mode samples and
                    statistics with a local log-evidence value greater than the
                    given bound. Default: -1.e90
                log_zero (float): Set a threshold value for which points with
                    a loglikelihood less than the given value will be ignored
                    by MultiNest. Default: -1e100
                max_iter (int): Set the maximum number of nested sampling
                    iterations performed by MultiNest. If 0, then it is
                    unlimited and MultiNest will stop using a different
                    criterion. Default: 0

    References:
        1. Feroz, Farhan, and M. P. Hobson. "Multimodal nested sampling: an
            efficient and robust alternative to Markov Chain Monte Carlo
            methods for astronomical data analyses." Monthly Notices of the
            Royal Astronomical Society 384.2 (2008): 449-463.
        2. Feroz, F., M. P. Hobson, and M. Bridges. "MultiNest: an efficient
            and robust Bayesian inference tool for cosmology and particle
            physics." Monthly Notices of the Royal Astronomical Society 398.4
            (2009): 1601-1614.
        3. Feroz, F., et al. "Importance nested sampling and the MultiNest
            algorithm." arXiv preprint arXiv:1306.2144 (2013).
        4. Buchner, J., et al. "X-ray spectral modelling of the AGN obscuring
            region in the CDFS: Bayesian model selection and catalogue."
            Astronomy & Astrophysics 564 (2014): A125.
    """

    def __init__(self, sampled_parameters, loglikelihood, population_size,
                 **multinest_kwargs):
        """Initialize the MultiNest Nested Sampler."""
        self.sampled_parameters = sampled_parameters
        self.loglikelihood = loglikelihood
        self.population_size = population_size
        self.multinest_kwargs = multinest_kwargs

        self._nDims = len(sampled_parameters)
        self._nDerived = 0
        self._output = None
        self._post_eval = False
        #if self.population_size is None:
        #    self.population_size = 25*self._nDims

        # Make the prior function for PyMultiNest.
        def prior(hypercube):
            return np.array([self.sampled_parameters[i].invcdf(value) for i,value in enumerate(hypercube)])

        self._prior = prior
        # multinest settings
        self._file_root = 'multinest_run_' #string

        return

    def run(self, verbose=False):
        """Initiate the MultiNest Nested Sampling run."""
        output = solve(LogLikelihood=self.loglikelihood, Prior=self._prior,
                       n_dims = self._nDims,
                       n_live_points=self.population_size,
                       outputfiles_basename=self._file_root,
                       verbose=verbose,
                       **self.multinest_kwargs)
        self._output = output
        return self.log_evidence, self.log_evidence_error

    @property
    def evidence(self):
        """float: Estimate of the Bayesian evidence, or Z."""
        return np.exp(self._output['logZ'])
    @evidence.setter
    def evidence(self, value):
        warnings.warn("evidence is not settable")

    @property
    def evidence_error(self):
        """float: Estimate (rough) of the error in the evidence, or Z."""
        return np.exp(self._output['logZerr'])
    @evidence_error.setter
    def evidence_error(self, value):
        warnings.warn("evidence_error is not settable")

    @property
    def log_evidence(self):
        """float: Estimate of the natural logarithm of the Bayesian evidence, or ln(Z).
        """
        return self._output['logZ']
    @log_evidence.setter
    def log_evidence(self, value):
        warnings.warn("log_evidence is not settable")

    @property
    def log_evidence_error(self):
        """float: Estimate of the error in the natural logarithm of the evidence.
        """
        return self._output['logZerr']
    @log_evidence_error.setter
    def log_evidence_error(self, value):
        warnings.warn("log_evidence_error is not settable")

    @property
    def information(self):
        """None: Not implemented yet->Estimate of the Bayesian information, or H."""
        return None
    @information.setter
    def information(self, value):
        warnings.warn("information is not settable")

    @property
    def multinest_file_root(self):
        """str. The root name used for MultNest output files."""
        return self._file_root
    @multinest_file_root.setter
    def multinest_file_root(self, value):
        self._file_root = value
        return

    def posteriors(self, nbins=None):
        """Estimates of the posterior marginal probability distributions of each parameter.
        Returns:
            dict of tuple of (numpy.ndarray, numpy.ndarray, numpy.ndarray): The
                histogram estimates of the posterior marginal probability
                distributions. The returned dict is keyed by the sampled
                parameter names and each element is a tuple with
                (marginal_weights, bin_edges, bin_centers).
        """
        # Lazy evaluation at first call of the function and store results
        # so that subsequent calls don't have to recompute.
        #print('nbins', nbins)
        if not self._post_eval:
            # Here the samples are samples directly from the posterior
            # (i.e. equal weights).
            samples = self._output['samples']
            # Rice bin count selection
            if nbins is None:
                nbins = 2 * int(np.cbrt(len(samples)))
            # print('nbins', nbins)
            nd = samples.shape[1]
            self._posteriors = dict()
            for ii in range(nd):
                marginal, edge = np.histogram(samples[:,ii], density=True, bins=nbins)
                center = (edge[:-1] + edge[1:])/2.
                self._posteriors[self.sampled_parameters[ii].name] = (marginal, edge, center)
            self._post_eval = True

        return self._posteriors

    def max_loglikelihood(self):
        mn_data = Analyzer(len(self.sampled_parameters), self._file_root, verbose=False).get_data()
        log_ls = -0.5*mn_data[:,1]
        ml = log_ls.max()
        return ml

    def deviance_ic(self):
        """Estimate Deviance Information Criterion.
        This function estimates the Deviance Information Criterion (DIC) for the
        model simulated with Nested Sampling (NS). It does so by using the
        posterior distribution estimates computed from the NS outputs.
        The DIC formula is given by:
            DIC = p_D + D_bar,
        where p_D = D_bar - D(theta_bar), D_bar is the posterior average of
        the deviance D(theta)= -2*ln(L(theta)) with L(theta) the likelihood
        of parameter set theta, and theta_bar is posterior average parameter set.

        Returns:
            float: The DIC estimate.
        """
        mn_data = Analyzer(len(self.sampled_parameters), self._file_root, verbose=False).get_data()
        params = mn_data[:,2:]
        log_likelihoods = -0.5*mn_data[:,1]
        prior_mass = mn_data[:,0]
        norm_weights = (prior_mass*np.exp(log_likelihoods))/self.evidence
        nw_mask = np.isnan(norm_weights)
        if  np.any(nw_mask):
            return np.inf
        D_of_theta = -2.*log_likelihoods
        D_bar = np.average(D_of_theta, weights=norm_weights)
        theta_bar = np.average(params, axis=0, weights=norm_weights)
        D_of_theta_bar = -2. * self.loglikelihood(theta_bar)
        p_D = D_bar - D_of_theta_bar
        return p_D + D_bar

    def best_fit_likelihood(self):
        """Parameter vector with the maximum likelihood.
        Returns:
            numpy.array: The parameter vector.
        """
        mn_data = Analyzer(len(self.sampled_parameters), self._file_root, verbose=False).get_data()
        log_ls = -0.5*mn_data[:,1]
        midx = np.argmax(log_ls)
        ml = mn_data[midx][2:]
        return ml


import pybullet as p
import time
import pybullet_data
import math as m
import numpy as np
import matplotlib.pyplot as plt
from tqdm import tqdm
import random

def init_individual():
    return list(np.random.uniform(-0.5, 0.5, 12))

def init_population(n=1000):
    return [init_individual() for i in range(n)]

def choose_fittest(pop, k):

    physicsClient = p.connect(p.DIRECT)

    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0,0,-10)
    planeId = p.loadURDF("../plane/plane.urdf")

    robotStartPos = [0,0,0.2]
    robotStartOrientation = p.getQuaternionFromEuler([0,0,0])
    robotId = p.loadURDF("../../robot/PP/urdf/PP.urdf", robotStartPos, robotStartOrientation)
    mode = p.POSITION_CONTROL

    vars = {"pc": physicsClient,
            "plane": planeId,
            "rid": robotId,
            "mode": mode}

    eval_list = []
    for i in tqdm(pop, desc="calc fitness"):
        score = evaluate_fitness(i, vars, gui=False)
        eval_list.append((i, score))
    sl = [i for i in sorted(eval_list, key=lambda item: -item[1])] # sort in inverse order (higher better)

    p.disconnect()

    return sl[:k] # take k best of inversely sorted list

def mutate(v, p):
    nv = []
    for i in range(len(v)):
        if np.random.uniform(0, 1) <= p:
            nv.append(np.random.uniform(-0.5, 0.5))
        else:
            nv.append(v[i])
    return nv

def crossover(v1, v2):
    i = random.choice(list(range(len(v1))))
    new = v1[0:i] + v2[i:len(v2)]
    return new

def genetic(e=10, n=1000):

    pop = init_population(n=n)
    print("Population Initialized ({})".format(n))

    best_score = 0
    best_weights = None

    for i in range(e):
        print("Iter {}/{}".format(i+1, e))

        new_pop = []

        fit = choose_fittest(pop, 300)

        fit_scores = [j[1] for j in fit]
        fit_pop = [j[0] for j in fit]

        best_e_score = fit_scores[0]

        if best_e_score > best_score:
            best_score = best_e_score
            best_weights = fit_pop[0]

        print("Best Epoch Weights: {}".format(fit_pop[0]))
        print("Best Overall Weights: {}".format(best_weights))

        print("Best Epoch Score: {}".format(best_e_score))

        print("Average Epoch Score: {}".format(sum(fit_scores)/len(fit_scores)))

        print("Best Overall Score: {}".format(best_score))

        new_pop += fit_pop

        # get crossovers
        crosses = []
        for j in range(len(fit_pop)):
            for k in range(len(fit_pop)):
                if j==k:
                    continue
                crosses.append(crossover(fit_pop[j], fit_pop[k]))

        # add mutations
        for j in crosses:
            new_pop.append(mutate(j, p=0.01))

        # add 100 extra randos
        new_pop += init_population(n=100)

        pop = new_pop

        print("Population Reproduced ({})".format(len(new_pop)))

    return best_weights, best_score

def evaluate_fitness(v, vars, gui=False, steps=1000):
    # if gui:
    #     physicsClient = p.connect(p.GUI)
    # else:
    #

    physicsClient = vars["pc"]
    planeId = vars["plane"]
    robotId = vars["rid"]
    mode = vars["mode"]

    robotStartPos = [0,0,0.2]
    robotStartOrientation = p.getQuaternionFromEuler([0,0,0])
    p.resetBasePositionAndOrientation(robotId, robotStartPos, robotStartOrientation)

    number_of_joints = p.getNumJoints(robotId)

    timediff = 1./2400.

    # torque info based off of https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_dynamics.py
    # id_revolute_joints = [0, 9, 6, 3, 1, 10, 7, 4, 2, 11, 8, 5]
    id_revolute_joints = [1, 10, 7, 4, 2, 11, 8, 5]

    motor_labels = {0: "LEFT FRONT ARMPIT", 1: "LEFT FRONT SHOULDER", 2: "LEFT FRONT KNEE",
    				3:"RIGHT FRONT ARMPIT", 4: "RIGHT FRONT SHOULDER", 5:"RIGHT FRONT KNEE",
    				6:"LEFT BACK ARMPIT", 7:"LEFT BACK SHOULDER", 8:"LEFT BACK KNEE",
    				9:"RIGHT BACK ARMPIT", 10:"RIGHT BACK SHOULDER", 11:"RIGHT BACK KNEE"}

    maxz = 0

    for i in range (steps):
        p.stepSimulation()

        shoulders_one_1 = v[0] + v[1]*m.sin(i*0.1 + v[2])
        p.setJointMotorControl2(robotId, 1, controlMode=mode, targetPosition=shoulders_one_1)
        p.setJointMotorControl2(robotId, 10, controlMode=mode, targetPosition=shoulders_one_1)

        shoulders_two_1 = v[3] + v[4]*m.sin(i*0.1 + v[5])
        p.setJointMotorControl2(robotId, 7, controlMode=mode, targetPosition=shoulders_two_1)
        p.setJointMotorControl2(robotId, 4, controlMode=mode, targetPosition=shoulders_two_1)

        knees_one_1 = v[6] + v[7]*m.sin(i*0.1 + v[8])
        p.setJointMotorControl2(robotId, 2, controlMode=mode, targetPosition=knees_one_1)
        p.setJointMotorControl2(robotId, 11, controlMode=mode, targetPosition=knees_one_1)

        knees_two_1 = v[9] + v[10]*m.sin(i*0.1 + v[11])
        p.setJointMotorControl2(robotId, 8, controlMode=mode, targetPosition=knees_two_1)
        p.setJointMotorControl2(robotId, 5, controlMode=mode, targetPosition=knees_two_1)

        robotPos, robotOrn = p.getBasePositionAndOrientation(robotId)

        if robotPos[2] > maxz:
            maxz = robotPos[2]

        if gui:
            time.sleep(timediff)
        else:
            pass

    robotPos, robotOrn = p.getBasePositionAndOrientation(robotId)
    score = np.linalg.norm(np.array([robotPos[0], robotPos[1]]))/maxz
    return score

if __name__ == "__main__":

    weights, score = genetic(e=10000, n=1000)

    physicsClient = p.connect(p.GUI)

    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    p.setGravity(0,0,-10)
    planeId = p.loadURDF("../plane/plane.urdf")

    robotStartPos = [0,0,0.2]
    robotStartOrientation = p.getQuaternionFromEuler([0,0,0])
    robotId = p.loadURDF("../../robot/PP/urdf/PP.urdf", robotStartPos, robotStartOrientation)
    mode = p.POSITION_CONTROL

    vars = {"pc": physicsClient,
            "plane": planeId,
            "rid": robotId,
            "mode": mode}

    score = evaluate_fitness(weights, vars, gui=True, steps=10000)

    p.disconnect()


#                             -*- Mode: Python -*-
# PathQuant.py
#

import numpy as np
import logging as log
import sys

class Path:
    """Path object. Wraps path data into a printable format.

    Methods
    -------
    get_min_cov
    get_sequence
    get_variant_name
    output_header
    """

    def __init__(self, db_f, ref_name, variant_name, ratio, expression,
                 min_coverage, start_off, sequence, ref_ratio, ref_expression,
                 ref_sequence, note):
        self.db_name = db_f
        self.ref_name = ref_name
        self.variant_name = variant_name
        self.rVAF = ratio
        self.expression = expression
        self.min_coverage = min_coverage
        self.start_off = start_off
        self.sequence = sequence
        self.ref_ratio = ref_ratio
        self.ref_expression = ref_expression
        self.ref_sequence = ref_sequence
        self.note = note

    def __str__(self):
        return "%s\t%s\t%s\t%.3f\t%.1f\t%d\t%d\t%s\t%.1f\t%s\t%s" % (
            self.db_name,
            self.ref_name,
            self.variant_name,
            self.rVAF,
            self.expression,
            self.min_coverage,
            self.start_off,
            self.sequence,
            self.ref_expression,
            self.ref_sequence,
            self.note)

    def __list__(self):
        return self.__str__().split('\t')

    def __getitem__(self, i):
        return self.__list__()[i]

    def get_min_cov(self):
        """Return minimum coverage"""

        return self.min_coverage

    def get_sequence(self):
        """Return variant sequence"""

        return self.sequence

    def get_variant_name(self):
        """Return variant description (also called variant name)"""

        return self.variant_name

    @staticmethod
    def output_header():
        """Output pre-formatted header"""

        sys.stdout.write("\t".join([
                "Database",
                "Query",
                "Type",
                "Variant_name",
                "rVAF",
                "Expression",
                "Min_coverage",
                "Start_offset",
                "Sequence",
                "Reference_expression",
                "Reference_sequence",
                "Info"
            ]) + "\n"
        )

class PathQuant:
    def __init__(self, all_paths, counts):
        self.all_paths = all_paths
        self.nb_kmer = len(counts)
        self.nb_seq = len(all_paths)

        self.counts = np.array(counts, dtype=np.float32)
        self.contrib = np.zeros((self.nb_kmer, self.nb_seq), dtype=np.int32)
        for seq_i, seq in enumerate(all_paths):
            for i in seq:
                self.contrib[i, seq_i] += 1
            # note: self.contrib[seq, seq_i] += 1 would not work in the case of ITDs

        self.coef = None
        self.rVAF = None

        log.info("%d sequence(s) are observed.", self.nb_seq)

    def compute_coef(self):
        # Set coefficient to zero if all paths use a kmer with 0 coverage
        #if self.contrib[self.counts == 0].min(axis=1).max() == 1:
        #    self.coef = np.zeros((len(c), 1), dtype=np.float32)
        #    return
        (coef, residual, rank, s) = np.linalg.lstsq(self.contrib, self.counts, rcond=None)
        self.coef = coef
        log.debug("Linear fitting = %s", self.coef)

    def refine_coef(self):
        # if max(self.coef) == 0: return
        # applies a gradient descent to get rid of negative coefficients
        self.coef[self.coef < 0] = 0
        last_max_grad = np.inf

        # convergence threshold
        num_iter = 0
        while last_max_grad > 0.01:
            counts_hat = np.dot(self.contrib, self.coef)
            grad = 2 * (self.counts - counts_hat) * self.contrib.T
            grad = grad.sum(axis=1) / self.nb_kmer
            self.coef += 0.1 * grad
            grad[self.coef < 0] = 0
            self.coef[self.coef < 0] = 0
            last_max_grad = np.max(np.abs(grad))
            num_iter += 1
            log.debug(
                "Iteration = %d, max_gradient = %f",
                num_iter,
                last_max_grad
            )
        log.info("Refined fitting = %s", self.coef)

    def get_ratio(self):
        if max(self.coef) == 0:
            self.rVAF = self.coef
        else:
            self.rVAF = self.coef / np.sum(self.coef)
        return self.rVAF

    def adjust_for_reference(self):
        self.rVAF[0] = np.nan
        self.rVAF[1] = np.nan
        self.coef[self.coef >= 0] = min(self.counts)