Skip to content
Snippets Groups Projects
Select Git revision
  • 0053d0900b66d99142a6edd0b1d719f081a96d7e
  • master default
2 results

RobotTests.scala

Blame
  • moveit_grasp_mediator.cpp 25.04 KiB
    #include "impl/moveit_grasp_mediator.h"
    #include <regex>
    #include <gb_grasp/MapConfigLoader.h>
    #include <moveit/robot_state/conversions.h>
    #include "impl/wing_moveit_decorator.h"
    
    
    void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
    	robots_.push_back(robot);
    	ROS_INFO("%s connected...", robot->name().c_str());
    	auto mr = dynamic_cast<Moveit_robot*>(robot);
    
    	manipulate_grasp_data(mr);
    
    	auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_);
    	double min_quality, max_candidates;
    	bool top_only;
    
      rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
    	rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates);
    	rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only);
    
      ROS_INFO("min_quality: %f", min_quality);
      ROS_INFO("max_candidate_count: %f", max_candidates);
      ROS_INFO("top_grasps_only: %f", top_only);
    
    
      map_gen->SetQuality(min_quality);
      map_gen->SetMaxGrasp(max_candidates);
    	map_gen->SetZGraspOnly(top_only);
    	mr->set_grasp_map_generator(map_gen);
    
    }
    
    void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
        double r,p,y;
        std::vector<double> rot_mapspace(3,0);
    
        tf2::Matrix3x3 m(tf.getRotation());
        m.getRPY(r,p, y);
        nh_->setParam("/mapspace/rot", std::vector<double>({r,p,y}));
        nh_->setParam("/mapspace/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), 0.91f}));
        nh_->setParam("/mapspace/dim", std::vector<double>({size.getX(), size.getY(), size.getZ()}));
        //nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f}));
        //nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()}));
        //nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y}));
    }
    
    void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
    	XmlRpc::XmlRpcValue data;
    	nh_->getParam("/", data);
    	
    	std::regex rx("panda_arm([0-9]+)");
        std::smatch match;
    	std::regex_match(robot->name(), match, rx);
    	data["base_link"]= "world";
    
    	data[robot->map().at("eef_name").c_str()]["end_effector_name"] = robot->map().at("eef_name").c_str();
    	data[robot->map().at("eef_name").c_str()]["end_effector_type"] = "finger";
    
    	data[robot->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001;
    	data[robot->map().at("eef_name").c_str()]["angle_resolution"] = 45;
    
    	data[robot->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035;
    	data[robot->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01;
    
    	data[robot->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005;
    
    	data[robot->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05;
    	data[robot->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05;
    	data[robot->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02;
    
    	data[robot->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005;
    
    	std::stringstream ss;
    	ss << "panda_" <<  match[1] << "_tool_center_point";
    	data[robot->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
    	data[robot->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
    	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
    	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
    	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105;
    	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
    	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
    	data[robot->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853;
    
    	data[robot->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
    	data[robot->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;
    
    	data[robot->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04;
    	data[robot->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
    	data[robot->map().at("eef_name").c_str()]["joints"][0] = robot->mgi_hand()->getJoints()[1].c_str();
    
    	data[robot->map().at("eef_name").c_str()]["max_finger_width"] = 0.085;
    	data[robot->map().at("eef_name").c_str()]["min_finger_width"] = 0.06;
    
    	data[robot->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08;
    
    	data[robot->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015;
    
    	data[robot->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022;
    	data[robot->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012;
    
    	data[robot->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
    	data[robot->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
    	nh_->setParam("/", data);
    }
    
    bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth,
                                double& y_width, double& z_height)
      {
        // Generate random cuboid
        double xmin = 0.1;
        double xmax = 0.7;
        double ymin = -0.25;
        double ymax = 0.25;
        double zmin = 0.89;
        double zmax = 1.2;
        rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax);
    
        double cuboid_size_min = 0.01;
        double cuboid_size_max = 0.03;
        rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max);
    
        object_name = "pick_target";
        visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds);
        visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED);
        visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM);
        visual_tools_->trigger();
    
        bool success = true;
        double timeout = 5;  // seconds
        ros::Rate rate(100);
        while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name))
        {
          rate.sleep();
          success = rate.cycleTime().toSec() < timeout;
        }
        return success;
    }
    
    void Moveit_grasp_mediator::mediate(){
    	visual_tools_->deleteAllMarkers();
    
      ROS_INFO("ns is %s", grasp_ns_.c_str());
      MapConfigLoader map_config(*nh_, grasp_ns_.c_str());
      auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
      map_config.SetObjects(cbd);
      float object_offset = 0.02f;
    
      tf2::Vector3 middle_pt(0,0,0);
      std::vector<double> x;
      std::vector<double> y;
      for(auto* ar: robots_){
        Moveit_robot* r = dynamic_cast<Moveit_robot*>(ar);
        tf2::Vector3 pos = r->tf().getOrigin();
        pos.setZ((pos.getZ()*2) +object_offset);
        tf2::Transform man_tf(r->tf().getRotation(), pos);
        manipulate_mapspace(man_tf, r->size());
        map_config.Create(grasp_maps_);
        middle_pt+= pos;
        ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
    
        tf2::Transform A = r->tf() * r->bounds()[0];
        tf2::Transform B = r->tf() * r->bounds()[1];
        tf2::Transform C = r->tf() * r->bounds()[2];
        tf2::Transform D = r->tf() * r->bounds()[3];
        x.push_back(A.getOrigin().getX());
        x.push_back(B.getOrigin().getX());
        x.push_back(C.getOrigin().getX());
        x.push_back(D.getOrigin().getX());
        y.push_back(A.getOrigin().getY());
        y.push_back(B.getOrigin().getY());
        y.push_back(C.getOrigin().getY());
        y.push_back(D.getOrigin().getY());
    
        r->grasp_map_generator()->SampleMap(grasp_maps_.front());
        for (auto* are: r->observers()){
          auto wmd = dynamic_cast<Wing_moveit_decorator*>(are);
          auto w = dynamic_cast<Wing*>(wmd->wing());
          pos.setX(w->world_tf().getOrigin().getX());
          pos.setY(w->world_tf().getOrigin().getY());
          man_tf.setRotation(w->world_tf().getRotation());
          man_tf.setOrigin(pos);
          manipulate_mapspace(man_tf, w->size());
          map_config.Create(grasp_maps_);
          middle_pt+= pos;
          ROS_INFO("X: %f, Y: %f, Z: %f", pos.getX(), pos.getY(), pos.getZ());
    
          A = w->world_tf() * w->bounds()[0];
          B = w->world_tf() * w->bounds()[1];
          C = w->world_tf() * w->bounds()[2];
          D = w->world_tf() * w->bounds()[3];
          x.push_back(A.getOrigin().getX());
          x.push_back(B.getOrigin().getX());
          x.push_back(C.getOrigin().getX());
          x.push_back(D.getOrigin().getX());
          y.push_back(A.getOrigin().getY());
          y.push_back(B.getOrigin().getY());
          y.push_back(C.getOrigin().getY());
          y.push_back(D.getOrigin().getY());
          r->grasp_map_generator()->SampleMap(grasp_maps_.front());
        }
      }
      ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
      middle_pt/= grasp_maps_.size();
      ROS_INFO("X: %f, Y: %f, Z: %f", middle_pt.getX(), middle_pt.getY(), middle_pt.getZ());
      auto x_min = *std::min_element(x.begin(),x.end());
      auto x_max = *std::max_element(x.begin(),x.end());
      auto y_min = *std::min_element(y.begin(),y.end());
      auto y_max = *std::max_element(y.begin(),y.end());
      ROS_INFO("width: %f", x_max-x_min);
      ROS_INFO("height: %f", y_max-y_min);
    
      visual_tools_->setAlpha(0.1);
      voxel_manager_->Voxelize();
    
      auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
      for(auto&o:cod){
        voxel_manager_->GetCuboidCollisions(o,voxel_environment_);
      }
      voxel_manager_->Voxelize(true);
        
    
        // -----------------------------------
        // Generate random object to grasp
        geometry_msgs::Pose object_pose;
        double object_x_depth;
        double object_y_width;
        double object_z_height;
        std::string object_name;
        if (!generateRandomCuboid(object_name, object_pose, object_x_depth, object_y_width, object_z_height))
        {
          ROS_INFO("Failed to add random cuboid ot planning scene");
        }
    
        // -----------------------------------
        // Generate grasp candidates
        std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates;
    
        // Configure the desired types of grasps
        moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config =
            moveit_grasps::TwoFingerGraspCandidateConfig();
        grasp_generator_config.disableAll();
        grasp_generator_config.enable_face_grasps_ = true;
        grasp_generator_config.generate_y_axis_grasps_ = true;
        grasp_generator_config.generate_x_axis_grasps_ = true;
        grasp_generator_config.generate_z_axis_grasps_ = true;
    
        auto grasp_generator_ =std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(visual_tools_);
    
        // Set the ideal grasp orientation for scoring
        std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 };
        grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy);
    
        // Set custom grasp score weights
        auto grasp_score_weights = std::make_shared<moveit_grasps::TwoFingerGraspScoreWeights>();
        grasp_score_weights->orientation_x_score_weight_ = 2.0;
        grasp_score_weights->orientation_y_score_weight_ = 2.0;
        grasp_score_weights->orientation_z_score_weight_ = 2.0;
        grasp_score_weights->translation_x_score_weight_ = 1.0;
        grasp_score_weights->translation_y_score_weight_ = 1.0;
        grasp_score_weights->translation_z_score_weight_ = 1.0;
        // Finger gripper specific weights.
        grasp_score_weights->depth_score_weight_ = 2.0;
        grasp_score_weights->width_score_weight_ = 2.0;
        // Assign the grasp score weights in the grasp_generator
        grasp_generator_->setGraspScoreWeights(grasp_score_weights);
        auto grasp_data_ = std::make_shared<moveit_grasps::TwoFingerGraspData>(*nh_, "hand_1", visual_tools_->getRobotModel());
        if (!grasp_data_->loadGraspData(*nh_, "hand_1"))
        {
          ROS_INFO("Failed to load Grasp Data parameters.");
          exit(-1);
        }
    
        grasp_generator_->setGraspCandidateConfig(grasp_generator_config);
        if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), object_x_depth, object_y_width,
                                              object_z_height, grasp_data_, grasp_candidates))
        {
          ROS_INFO("Grasp generator failed to generate any valid grasps");
        }
    
        // --------------------------------------------
        // Generating a seed state for filtering grasps
        auto seed_state = std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
        Eigen::Isometry3d eef_mount_grasp_pose =
            visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse();
        if (!getIKSolution(robot_model_->getJointModelGroup("panda_arm1"), eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName()))
        {
          ROS_INFO("The ideal seed state is not reachable. Using start state as seed.");
        }
    
        // --------------------------------------------
        // Filtering grasps
        // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates)
        bool filter_pregrasps = true;
        auto grasp_filter_ = std::make_shared<moveit_grasps::TwoFingerGraspFilter>(visual_tools_->getSharedRobotState(), visual_tools_);
        if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, robot_model_->getJointModelGroup("panda_arm1"), seed_state, filter_pregrasps,
                                         object_name))
        {
          ROS_INFO("Filter grasps failed");
        }
        if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates))
        {
          ROS_INFO("Grasp filtering removed all grasps");
        }
    
        // Plan free-space approach, cartesian approach, lift and retreat trajectories
        moveit_grasps::GraspCandidatePtr selected_grasp_candidate;
        moveit_msgs::MotionPlanResponse pre_approach_plan;
        if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name))
        {
          ROS_INFO("Failed to plan grasp motions");
        }
    
        visualizePick(selected_grasp_candidate, pre_approach_plan);
    }
    
      void Moveit_grasp_mediator::visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
                         const moveit_msgs::MotionPlanResponse& pre_approach_plan)
      {
        EigenSTL::vector_Isometry3d waypoints;
        moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints);
    
        // Visualize waypoints
        visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp");
        visual_tools_->publishAxisLabeled(waypoints[1], "grasp");
        visual_tools_->publishAxisLabeled(waypoints[2], "lifted");
        visual_tools_->publishAxisLabeled(waypoints[3], "retreat");
        visual_tools_->trigger();
    
        // Get the pre and post grasp states
        visual_tools_->prompt("pre_grasp");
        robot_state::RobotStatePtr pre_grasp_state =
            std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
        valid_grasp_candidate->getPreGraspState(pre_grasp_state);
        visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE);
        robot_state::RobotStatePtr grasp_state =
            std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
        if (valid_grasp_candidate->getGraspStateClosed(grasp_state))
        {
          visual_tools_->prompt("grasp");
          visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW);
        }
        if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 &&
            valid_grasp_candidate->segmented_cartesian_traj_[1].size())
        {
          visual_tools_->prompt("lift");
          visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(),
                                           rviz_visual_tools::BLUE);
        }
        if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 &&
            valid_grasp_candidate->segmented_cartesian_traj_[2].size())
        {
          visual_tools_->prompt("retreat");
          visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(),
                                           rviz_visual_tools::PURPLE);
        }
    
        visual_tools_->prompt("show free space approach");
        visual_tools_->hideRobot();
        visual_tools_->trigger();
    
        bool wait_for_animation = true;
        visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation);
        ros::Duration(0.25).sleep();
        if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::APPROACH)
          visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH],
                                               valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
        ros::Duration(0.25).sleep();
    
        if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::LIFT)
          visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT],
                                               valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
        ros::Duration(0.25).sleep();
    
        if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::RETREAT)
          visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT],
                                               valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
        ros::Duration(0.25).sleep();
    }
    
    bool Moveit_grasp_mediator::planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
                         moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
                         moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name)
      {
        moveit::core::RobotStatePtr current_state;
        {
          boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
              new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
          current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState());
        }
    
        std::shared_ptr<moveit_grasps::GraspPlanner>grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_);
    
        // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in
        grasp_planner_->setWaitForNextStepCallback(boost::bind(&Moveit_grasp_mediator::waitForNextStep, this, visual_tools_, _1));
    
        bool success = false;
        for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin()))
        {
          valid_grasp_candidate = grasp_candidates.front();
          valid_grasp_candidate->getPreGraspState(current_state);
          if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false,
                                                       object_name))
          {
            ROS_INFO("failed to plan approach lift retreat");
            continue;
          }
    
          robot_state::RobotStatePtr pre_grasp_state =
              valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front();
          if (!planPreApproach(*pre_grasp_state, pre_approach_plan))
          {
            ROS_INFO("failed to plan to pregrasp_state");
            continue;
          }
    
          success = true;
          break;
        }
        return success;
    }
    
    void Moveit_grasp_mediator::waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt)
    {
      visual_tools->prompt(prompt);
    }
    
    bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan)
      {
        planning_interface::MotionPlanRequest req;
        planning_interface::MotionPlanResponse res;
    
        double tolerance_above = 0.01;
        double tolerance_below = 0.01;
        // req.planner_id = "RRTConnectkConfigDefault";
        req.group_name = "panda_1_arm1";
        req.num_planning_attempts = 5;
        req.allowed_planning_time = 1.5;
        moveit_msgs::Constraints goal =
            kinematic_constraints::constructGoalConstraints(goal_state, robot_model_->getJointModelGroup("panda_arm1"), tolerance_below, tolerance_above);
    
        req.goal_constraints.push_back(goal);
        boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
            new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
    
        // ---------------------------------
        // Change the robot current state
        // NOTE: We have to do this since Panda start configuration is in self collision.
        robot_state::RobotState rs = (*ls)->getCurrentState();
        std::vector<double> starting_joint_values = { 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
        std::vector<std::string> joint_names = { "panda_1_joint1", "panda_1_joint2", "panda_1_joint3", "panda_1_joint4",
                                                 "panda_1_joint5", "panda_1_joint6", "panda_1_joint7" };
        // arm_jmg_->getActiveJointModelNames();
        for (std::size_t i = 0; i < joint_names.size(); ++i)
        {
          rs.setJointPositions(joint_names[i], &starting_joint_values[i]);
        }
        rs.update();
        robot_state::robotStateToRobotStateMsg(rs, req.start_state);
        // ---------------------------
    
        planning_pipeline_->generatePlan(*ls, req, res);
        if (res.error_code_.val != res.error_code_.SUCCESS)
        {
          ROS_INFO( "Failed to plan approach successfully");
          return false;
        }
    
        res.getMessage(pre_approach_plan);
        return true;
    }
    
    bool Moveit_grasp_mediator::isStateValid(const planning_scene::PlanningScene* planning_scene,
                      const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
                      const robot_model::JointModelGroup* group, const double* ik_solution)
    {
      robot_state->setJointGroupPositions(group, ik_solution);
      robot_state->update();
      return !planning_scene->isStateColliding(*robot_state, group->getName());
    }
    
    bool Moveit_grasp_mediator::getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose,
                         robot_state::RobotState& solution, const std::string& link_name)
      {
        boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
            new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
    
        moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind(
            &Moveit_grasp_mediator::isStateValid,this, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(), visual_tools_, _1, _2, _3);
    
        // seed IK call with current state
        solution = (*ls)->getCurrentState();
    
        // Solve IK problem for arm
        // disable explicit restarts to guarantee close solution if one exists
        const double timeout = 0.1;
        return solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn);
    }
    
    void Moveit_grasp_mediator::scene_setup(){
    	for(Abstract_robot* ar: robots_) ar->notify();
        visual_tools_->setAlpha(1.0);
    	  auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
        auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
    
    	  ROS_INFO("%i die cuboids", cbd.size());
    	  ROS_INFO("%i cuboids", cod.size());
    
        for(auto&o:cod){
          o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
          psi_->addCollisionObjects({o.getCoMsg()});
        }
    
        for(auto&o:cbd){
    		//o.publish(visual_tools_,rviz_visual_tools::GREEN);
    		  psi_->addCollisionObjects({o.getCoMsg()});
    	  }
    
    	visual_tools_->publishRobotState(mgi_->getCurrentState());
    	visual_tools_->trigger();
            
    }
    
    Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
    	: Moveit_mediator(objects, pub, nh)
        , robot_reader_(std::make_unique<Robot_reader>(nh))
        , wing_reader_(std::make_unique<Wing_reader>(nh))
        , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
        // , job_reader_(std::make_unique<Job_reader>(nh)) {
            visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
            robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
            visual_tools_->loadMarkerPub();
            visual_tools_->loadRemoteControl();
            visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
    
            visual_tools_->loadRobotStatePub("/display_robot_state");
            visual_tools_->setRobotStateTopic("/robot_state_publisher");
            visual_tools_->loadTrajectoryPub("/display_planned_path");
    
            visual_tools_->loadSharedRobotState();
            visual_tools_->enableBatchPublishing();
            visual_tools_->deleteAllMarkers();
            visual_tools_->removeAllCollisionObjects();
            visual_tools_->trigger();
    
            nh_->getParam("/grasps_ns", grasp_ns_);
            grasp_ns_ = "grasps_ns";
            ROS_INFO("ns is %s", grasp_ns_.c_str());
    
            planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter");
            voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_);
    
            auto rd = robot_reader_->robot_data();
            for (int i = 0; i < rd.size() ;i++) connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
    
            auto wd = static_cast<Wing_reader*>(wing_reader_.get())->wing_data();
            for (int i = 0; i < robots_.size(); i++){
                for(object_data& w : wd[i].first){
                    w.pose_ = robots_[i]->tf().inverse() * w.pose_;
                }
                robots_[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
            }
    
            set_wings(wd);    
    
            for (int i = 0; i < robots_.size(); i++){
                std::bitset<3> bs = wd[i].second;
                build_wings(bs, i);
            }
    
            for (auto* r : robots_)r->notify();
    
            publish_tables();
            scene_setup();
        };