new map data

src/ceti/launch/run_benchmark_ompl.launch

+<launch>
+
+  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
+  <arg name="cfg" />
+
+  <!-- Load URDF -->
+  <include file="$(dirname)/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- Start the database -->
+  <include file="$(dirname)/warehouse.launch">
+    <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
+  </include>
+
+  <!-- Start Benchmark Executable -->
+  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
+    <rosparam command="load" file="$(find ceti)/config/ompl_planning.yaml"/>
+  </node>
+
+</launch>

src/ceti/launch/sensor_manager.launch.xml

+<launch>
+
+  <!-- This file makes it easy to include the settings for sensor managers -->
+
+  <!-- Params for 3D sensors config -->
+  <rosparam command="load" file="$(find ceti)/config/sensors_3d.yaml" />
+
+  <!-- Params for the octomap monitor -->
+  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
+  <param name="octomap_resolution" type="double" value="0.025" />
+  <param name="max_range" type="double" value="5.0" />
+
+  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
+  <arg name="moveit_sensor_manager" default="panda" />
+  <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
+
+</launch>

src/ceti/launch/setup_assistant.launch

+<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
+<launch>
+
+  <!-- Debug Info -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <!-- Run -->
+  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
+        args="--config_pkg=ceti"
+        launch-prefix="$(arg launch_prefix)"
+        required="true"
+        output="screen" />
+
+</launch>

src/ceti/launch/simple_moveit_controller_manager.launch.xml

+<launch>
+  <!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
+  <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+
+  <!-- Load controller list to the parameter server -->
+  <rosparam file="$(find ceti)/config/simple_moveit_controllers.yaml" />
+  <rosparam file="$(find ceti)/config/ros_controllers.yaml" />
+</launch>

src/ceti/launch/stomp_planning_pipeline.launch.xml

+<launch>
+  <!-- Stomp Plugin for MoveIt -->
+  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <arg name="jiggle_fraction" value="0.05" />
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints" />
+
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+  <!-- Add MoveGroup capabilities specific to this pipeline -->
+  <!-- <param name="capabilities" value="" /> -->
+
+  <rosparam command="load" file="$(find ceti)/config/stomp_planning.yaml"/>
+</launch>

src/ceti/launch/trajectory_execution.launch.xml

+<launch>
+  <!-- This file summarizes all settings required for trajectory execution  -->
+
+  <!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" />
+  <arg name="fake_execution_type" default="interpolate" />
+
+  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
+  <arg name="moveit_manage_controllers" default="true"/>
+  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
+
+  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
+  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
+  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
+  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
+
+  <!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
+       As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
+  <include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
+
+</launch>

src/ceti/launch/warehouse.launch

+<launch>
+
+  <!-- The path to the database must be specified -->
+  <arg name="moveit_warehouse_database_path" />
+
+  <!-- Load warehouse parameters -->
+  <include file="$(dirname)/warehouse_settings.launch.xml" />
+
+  <!-- Run the DB server -->
+  <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
+    <param name="overwrite" value="false"/>
+    <param name="database_path" value="$(arg moveit_warehouse_database_path)" />
+  </node>
+
+</launch>

src/ceti/launch/warehouse_settings.launch.xml

+<launch>
+  <!-- Set the parameters for the warehouse and run the mongodb server. -->
+
+  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
+  <arg name="moveit_warehouse_port" default="33829" />
+
+  <!-- The default DB host for moveit -->
+  <arg name="moveit_warehouse_host" default="localhost" />
+
+  <!-- Set parameters for the warehouse -->
+  <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
+  <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
+  <param name="warehouse_exec" value="mongod" />
+  <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
+
+</launch>

src/ceti/package.xml

+<package>
+
+  <name>ceti</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the panda with the MoveIt Motion Planning Framework
+  </description>
+  <author email="matteo.aneddama@gmail.com">matteo</author>
+  <maintainer email="matteo.aneddama@gmail.com">matteo</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_fake_controller_manager</run_depend>
+  <run_depend>moveit_kinematics</run_depend>
+  <run_depend>moveit_planners_ompl</run_depend>
+  <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>moveit_setup_assistant</run_depend>
+  <run_depend>moveit_simple_controller_manager</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>joint_state_publisher_gui</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>rviz</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>xacro</run_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+       We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <run_depend>joint_trajectory_controller</run_depend> -->
+  <!-- <run_depend>gazebo_ros_control</run_depend> -->
+  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
+  <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
+  <run_depend>franka_description</run_depend>
+
+
+</package>

src/mtc/launch/grasp_pipeline.launch

@@ -42,7 +42,7 @@
 
   <!-- Planning Functionality -->
   <arg name="planner" default="ompl" />
-  <include ns="moveit_grasps" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
+  <include ns="moveit_grasps" file="$(find ceti)/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="$(arg planner)" />
   </include>
 

src/mtc/maps/dummy.yaml

 data:
-  - -0.704647 -0.085013 0.103821 -0.063726 -0.039416 -0.249854 0.965380
-  - -0.513205 -0.514270 0.103184 0.092404 -0.010381 -0.590406 0.801732
-  - -0.499355 -0.290624 0.104579 -0.041985 -0.015622 -0.411819 0.910164
-  - -0.459296 -0.182886 0.049981 -0.221514 0.461441 -0.859037 0.007668
-  - -0.519670 0.093757 0.102952 0.057347 -0.080399 -0.305704 0.946991
-  - -0.499105 0.404992 0.000930 -0.557591 0.429723 0.428737 0.566228
-  - -0.489862 0.521042 0.102369 -0.020562 0.110022 0.800758 0.588437
-  - -0.294706 -0.580002 0.067799 0.390187 -0.157800 0.394885 0.816651
-  - -0.322003 -0.302647 0.102635 0.061764 -0.086307 -0.477962 0.871946
-  - -0.297281 -0.032005 0.079964 -0.311869 -0.148179 -0.436303 0.830916
-  - -0.323814 0.102339 0.102237 0.017354 -0.113384 -0.245576 0.962567
-  - -0.221989 0.339610 0.058054 -0.434513 -0.186412 -0.879328 0.056848
-  - -0.282236 0.566463 -0.079323 -0.842237 0.410323 0.066782 0.343238
-  - -0.045863 -0.717152 0.088317 -0.256995 0.115736 -0.952974 0.111348
-  - -0.017530 -0.503598 0.064890 -0.243688 0.362787 -0.533595 0.724071
-  - -0.087573 -0.290814 0.103857 -0.066561 0.031855 -0.467458 0.880930
-  - -0.162281 0.379039 0.029983 -0.367444 -0.471388 -0.191603 0.778503
-  - -0.001871 0.499820 0.037360 -0.225484 0.520819 -0.328508 0.754975
-  - 0.000220 0.731249 -0.002125 0.004754 0.714210 0.212787 0.666785
-  - 0.097747 -0.713925 0.104048 0.058142 0.033945 -0.634190 0.770241
-  - 0.000591 -0.480898 -0.027892 -0.540596 -0.583588 0.320502 0.514257
-  - 0.177063 -0.326099 0.066371 -0.179291 0.389617 -0.620917 0.656137
-  - 0.071622 -0.165081 0.077357 0.042348 -0.360331 0.804873 0.469626
-  - 0.175867 0.259083 -0.059958 -0.622893 0.630490 -0.442872 0.135467
-  - 0.071068 0.437589 0.079327 0.239442 0.254795 -0.889244 0.294944
-  - 0.252831 -0.566984 0.065675 0.121811 -0.415237 0.561186 0.705557
-  - 0.295272 -0.292902 0.104653 0.015052 -0.037756 -0.977591 0.206554
-  - 0.284065 -0.076036 0.100979 -0.071881 -0.118235 -0.419818 0.896999
-  - 0.382397 0.139887 0.051428 -0.304978 0.402606 -0.169316 0.846303
-  - 0.238413 0.384767 0.006826 -0.655008 -0.196111 0.241573 0.688584
-  - 0.237968 0.455913 0.072342 0.158007 -0.361317 0.187635 0.899598
-  - 0.582562 -0.238750 0.021376 -0.629042 0.050133 -0.584335 0.510240
-  - 0.515243 -0.097356 0.103854 -0.073570 0.006677 -0.963172 0.258540
-  - 0.548987 0.125685 0.089250 -0.243039 0.126222 -0.550080 0.788930
-  - 0.526708 0.200380 -0.019687 0.363156 -0.679607 0.620766 0.144572
-  - 0.545807 0.477673 0.091806 -0.188434 0.165296 -0.933886 0.255002
-  - 0.698731 -0.108383 0.104657 0.037677 0.014594 -0.496260 0.867233
-  - 0.688405 0.103068 0.104313 0.013921 -0.055488 -0.482543 0.874002
+  - -0.500000 -0.300000 0.885000 -0.000351 -0.587573 -0.809145 0.006419
+  - -0.500000 -0.100000 0.885000 0.399194 0.560541 -0.479417 0.544607
+  - -0.500000 0.100000 0.885000 -0.055008 -0.026243 -0.307218 0.949686
+  - -0.300000 -0.500000 0.885000 -0.189051 -0.725537 -0.142815 0.646111
+  - -0.300000 -0.300000 0.885000 -0.001376 0.000193 -0.452168 0.891932
+  - -0.300000 -0.100000 0.885000 0.002569 0.016521 0.999449 0.028674
+  - -0.300000 0.100000 0.885000 -0.356466 -0.452052 0.339010 0.744079
+  - -0.300000 0.300000 0.885000 -0.012014 -0.009510 -0.357271 0.933875
+  - -0.100000 -0.500000 0.885000 0.308517 -0.609275 -0.698421 0.214030
+  - -0.100000 -0.300000 0.885000 -0.751057 -0.513430 0.241023 0.337950
+  - -0.100000 -0.100000 0.885000 0.482030 -0.003684 0.433699 0.761274
+  - -0.100000 0.100000 0.885000 0.560023 -0.208890 -0.683158 0.419565
+  - -0.100000 0.100000 1.085000 -0.518113 -0.000194 -0.244317 0.819675
+  - -0.100000 0.300000 0.885000 0.464837 -0.321952 -0.708882 0.421615
+  - -0.100000 0.500000 0.885000 0.074026 0.072863 0.296321 0.949424
+  - 0.100000 -0.500000 0.885000 -0.007942 -0.069720 -0.932787 0.353531
+  - 0.100000 -0.300000 0.885000 0.018094 0.001651 -0.561943 0.826976
+  - 0.100000 -0.100000 0.885000 0.060155 -0.700045 0.318748 0.636174
+  - 0.100000 0.100000 0.885000 -0.247111 -0.179083 -0.641048 0.704218
+  - 0.100000 0.100000 1.085000 -0.188640 0.290960 -0.906098 0.242371
+  - 0.100000 0.300000 0.885000 -0.000001 0.000328 -0.386438 0.922315
+  - 0.100000 0.500000 0.885000 -0.551419 0.149555 -0.145163 0.807774
+  - 0.300000 -0.300000 0.885000 -0.778941 -0.031396 -0.206180 0.591400
+  - 0.300000 -0.100000 0.885000 -0.201027 0.728163 -0.264099 0.599682
+  - 0.300000 0.100000 0.885000 0.399720 -0.302392 0.254966 0.826907
+  - 0.300000 0.300000 0.885000 -0.248105 0.115392 -0.798538 0.536158
+  - 0.500000 -0.100000 0.885000 -0.201778 0.200151 -0.158703 0.945536
+  - 0.500000 0.100000 0.885000 0.014289 0.056052 -0.322179 0.944910
+  - 0.500000 0.300000 0.885000 -0.702128 -0.167470 0.189804 0.665541
 description:
   system: Franka Emika 'Panda'
   resolution: 0.2
-  dimension: 0.900000 0.900000 0.000000 (m)
-  timestamp: 1712.052411 sec
\ No newline at end of file
+  dimension: 0.900000 0.900000 1.200000 (m)
+  timestamp: 8995.084193 sec
\ No newline at end of file

src/mtc/src/grasp_pipeline.cpp

@@ -201,7 +201,7 @@ public:
     // --------------------------------------------s
     // Filtering grasps
     // Note: This step also solves for the grasp and pre-grasp states and stores them in gras
-
+  /*
   void visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
                      const moveit_msgs::MotionPlanResponse& pre_approach_plan)
   {
@@ -265,6 +265,7 @@ public:
                                            valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
     ros::Duration(0.25).sleep();
   }
+  */
 
   bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
                      moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
@@ -325,15 +326,6 @@ public:
     // 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_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
-                                             "panda_joint5", "panda_joint6", "panda_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);
     // ---------------------------
 
@@ -584,7 +576,7 @@ int main(int argc, char* argv[])
   double z_depth = 0.4;
   float dimension_x = 0.9;
   float dimension_y = 0.9;
-  float dimension_z = 0; //1.2;
+  float dimension_z = 1.2;
 
   float resolution = 0.2; 
 
@@ -593,10 +585,9 @@ int main(int argc, char* argv[])
   
   for (float x = 0 - dimension_x; x <= -0.22f + dimension_x; x+= resolution){
       for(float y = 0 - dimension_y; y <= 0 + dimension_y; y+= resolution){
-          for(float z = 0 - dimension_z; z <= 0.885f + dimension_z; z+= resolution){
-            //tester.reachGrasp(x,y,z, object_name, x_depth, y_depth, z_depth);
+          for(float z = 0.885f; z <= 0.885f + dimension_z; z+= resolution){
+            tester.reachGrasp(x,y,z, object_name, x_depth, y_depth, z_depth);
           }
-          tester.reachGrasp(x,y,dimension_z, object_name, x_depth, y_depth, z_depth);
       }
   }