diff --git a/include/impl/moveit_grasp_mediator.h b/include/impl/moveit_grasp_mediator.h
index aee350051ac2602c0a944bafee42d8a20243e1d6..b3f3856cd48507220e3f9b1bc314ff8666ed586f 100644
--- a/include/impl/moveit_grasp_mediator.h
+++ b/include/impl/moveit_grasp_mediator.h
@@ -11,6 +11,7 @@
#include "impl/abstract_robot.h"
#include "impl/robot.h"
#include <gb_grasp/VoxelManager.h>
+#include <gb_grasp/GraspMap.h>
#include "reader/abstract_param_reader.h"
#include "reader/robot_reader.h"
#include "reader/cuboid_reader.h"
@@ -22,6 +23,9 @@ class Moveit_grasp_mediator : public Moveit_mediator{
std::shared_ptr<planning_pipeline::PlanningPipeline> planning_pipeline_;
std::shared_ptr<VoxelManager> voxel_manager_;
std::string grasp_ns_;
+ boost::dynamic_bitset<> voxel_environment_;
+ std::vector<GraspMap> grasp_maps_;
+
// Readers
std::unique_ptr<Abstract_param_reader> robot_reader_;
@@ -29,53 +33,7 @@ class Moveit_grasp_mediator : public Moveit_mediator{
std::unique_ptr<Abstract_param_reader> cuboid_reader_;
public:
- 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)) {
-
- 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_);
-
- 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 = static_cast<Robot_reader*>(robot_reader_.get())->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);
- }
-
- scene_setup();
- };
+ Moveit_grasp_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
void mediate() override;
@@ -83,6 +41,21 @@ class Moveit_grasp_mediator : public Moveit_mediator{
void manipulate_grasp_data(Moveit_robot* robot);
void scene_setup();
+ void manipulate_mapspace();
+ bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth, double& y_width, double& z_height);
+ bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose, robot_state::RobotState& solution, const std::string& link_name);
+ bool 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);
+ void visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
+ const moveit_msgs::MotionPlanResponse& pre_approach_plan);
+ void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt);
+ bool planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan);
+ bool 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);
+
+
};
diff --git a/include/impl/moveit_mediator.h b/include/impl/moveit_mediator.h
index b6eeccd8abc8991869355b074cdf5ece6e7f96e9..e551a84d2050defb744ab4faca4ab3be8a96ff17 100644
--- a/include/impl/moveit_mediator.h
+++ b/include/impl/moveit_mediator.h
@@ -68,29 +68,7 @@ class Moveit_mediator : public Abstract_mediator{
public:
- Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
- : Abstract_mediator(objects, pub), nh_(nh)
- , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
- , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
- , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
- , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
- , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
- , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){
-
- robot_model_loader::RobotModelLoaderPtr robot_model_loader;
- robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
- robot_model_ = robot_model_loader->getModel();
- ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
-
- // planner
- sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
-
- // cartesian
- cartesian_planner_->setMaxVelocityScaling(1.0);
- cartesian_planner_->setMaxAccelerationScaling(1.0);
- cartesian_planner_->setStepSize(.01);
-
- };
+ Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh);
void setup_task();
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
diff --git a/include/impl/moveit_robot.h b/include/impl/moveit_robot.h
index 258632e7613022b74d2a05e79602852aa32502d3..ca49cc751d7330c98fb8fec078bc3b819713195d 100644
--- a/include/impl/moveit_robot.h
+++ b/include/impl/moveit_robot.h
@@ -39,6 +39,8 @@ class Moveit_robot : public Robot{
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;}
inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi_hand() {return mgi_hand_;}
+ inline std::shared_ptr<MapGenerator> grasp_map_generator() {return grasp_map_generator_;}
+
inline std::map<std::string, std::string>& map(){return map_;}
inline void set_grasp_map_generator(std::shared_ptr<MapGenerator>const& d ){ grasp_map_generator_ = d;}
diff --git a/include/reader/cuboid_reader.h b/include/reader/cuboid_reader.h
index f142d902857ec3ef99cdeed2a0e86259e0f400b0..7959ddbb543e04ee1534fb8082bda5353418e111 100644
--- a/include/reader/cuboid_reader.h
+++ b/include/reader/cuboid_reader.h
@@ -12,16 +12,16 @@
class Cuboid_reader : public Abstract_param_reader{
protected:
- std::vector<Cuboid> cuboid_bin_;
+ std::vector<Cuboid> cuboid_box_;
std::vector<Cuboid> cuboid_obstacle_;
public:
Cuboid_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){read();}
- inline void set_cuboid_bin_data(std::vector<Cuboid>& cuboid_data) {cuboid_bin_ = cuboid_data;}
+ inline void set_cuboid_bin_data(std::vector<Cuboid>& cuboid_data) {cuboid_box_ = cuboid_data;}
inline void set_cuboid_obstacle_data(std::vector<Cuboid>& cuboid_data) {cuboid_obstacle_ = cuboid_data;}
- inline std::vector<Cuboid> cuboid_bin() {return cuboid_bin_;}
+ inline std::vector<Cuboid> cuboid_bin() {return cuboid_box_;}
inline std::vector<Cuboid> cuboid_obstacle() {return cuboid_obstacle_;}
void read() override;
diff --git a/launch/mtc_auerswald.launch b/launch/mtc_auerswald.launch
index fb65cad9226fda15814ae4e53f0b336fdad55bda..7573c9c205fb52d81f0479a2a36562c98397cede 100644
--- a/launch/mtc_auerswald.launch
+++ b/launch/mtc_auerswald.launch
@@ -2,57 +2,64 @@
<!-- Launch rviz.launch before running this demo to see visalizations -->
<arg name="result" default="dummy/-562289182.yaml"/>
<arg name="grasps_ns" default="grasps" />
-
+ <arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" />
+ <arg name="planner" default="ompl" />
<arg name="moveit_controller_manager" default="fake" />
<arg name="fake_execution_type" default="interpolate" />
<arg name="use_gui" default="false" />
<arg name="use_rviz" default="true" />
+ <!-- GDB Debug Arguments -->
+ <arg name="debug" default="false" />
+ <arg unless="$(arg debug)" name="launch_prefix" value="" />
+ <arg if="$(arg debug)" name="launch_prefix"
+ value="gdb -x $(find moveit_grasps)/launch/gdb_settings.gdb --ex run --args" />
+ <!-- Verbose mode -->
+ <arg name="verbose" default="false" />
+ <arg unless="$(arg verbose)" name="command_args" value="" />
+ <arg if="$(arg verbose)" name="command_args" value="--verbose" />
- <!-- PANDA -->
- <include file="$(find ceti_double)/launch/planning_context.launch">
- <arg name="load_robot_description" value="true"/>
- <arg name="scene" value="$(arg result)" />
- </include>
+ <!-- Callgrind Arguments -->
+ <arg name="callgrind" default="false" />
+ <arg unless="$(arg callgrind)" name="launch_prefix2" value="" />
+ <arg if="$(arg callgrind)" name="launch_prefix2" value="valgrind --tool=callgrind --collect-atstart=no" />
- <!-- this is needed because moveit grasps uses an additional joint -->
- <arg name="robot_description" default="robot_description"/>
- <param name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/double_panda_arm_two_finger_hand.urdf.xacro' 'scene:=$(arg result)'"/>
+ <!-- Valgrind Arguments -->
+ <arg name="valgrind" default="false" />
+ <arg unless="$(arg valgrind)" name="launch_prefix3" value="" />
+ <arg if="$(arg valgrind)" name="launch_prefix3" value="valgrind --tool=memcheck --leak-check=full --show-leak-kinds=all --track-origins=yes -v" />
+ <!-- Publish world frame -->
+ <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base_1" />
+ <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_2" args="0 0 0 0 0 0 world base_2" />
- <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base_1" />
- <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base_2" />
+ <!-- PANDA -->
+ <include file="$(find moveit_grasps)/launch/panda_planning_context.launch">
+ <arg name="load_robot_description" value="true"/>
+ <arg name="gripper" value="$(arg gripper)"/>
+ <arg name="scene" value="$(arg result)"/>
+ </include>
- <!-- Load Rviz -->
- <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
- args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen">
- <rosparam command="load" file="$(find ceti_double)/config/kinematics.yaml"/>
- </node>
+ <!-- PANDA -->
+ <include file="$(find moveit_grasps)/launch/load_panda.launch">
+ <arg name="gripper" value="$(arg gripper)"/>
+ <arg name="scene" value="$(arg result)" />
+ </include>
- <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find ceti_double)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
<arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
<arg name="info" value="true"/>
- <arg name="debug" value="true"/>
- <arg name="pipeline" value="ompl" />
+ <arg name="debug" value="$(arg debug)"/>
+ <arg name="pipeline" value="$(arg planner)"/>
+ <arg name="load_robot_description" value="false"/>
<arg name="scene" value="$(arg result)" />
</include>
- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
- <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
- </node>
-
- <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
- <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
- </node>
-
- <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
<rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
@@ -66,8 +73,14 @@
<rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
</node>
+
+ <!-- Load Rviz -->
+ <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+ args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen">
+ </node>
+
+
<!-- Planning Functionality -->
- <arg name="planner" default="ompl" />
<include ns="grasp_cell_routine" file="$(find ceti_double)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="$(arg planner)" />
</include>
diff --git a/launch/rviz/cell_routine.rviz b/launch/rviz/cell_routine.rviz
index 02da6ef90bb6f74f21e5a7e92e620aed1a032e96..7ddb5d03657fe433c1b32652c1d1284d409a513d 100644
--- a/launch/rviz/cell_routine.rviz
+++ b/launch/rviz/cell_routine.rviz
@@ -45,6 +45,336 @@ Toolbars:
Visualization Manager:
Class: ""
Displays:
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /rviz_visual_tools
+ Name: MarkerArray
+ Namespaces:
+ Axis: true
+ Cuboid: true
+ Queue Size: 100
+ Value: true
+ - Class: moveit_rviz_plugin/Trajectory
+ Color Enabled: false
+ Enabled: true
+ Interrupt Display: false
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_1_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_tool_center_point:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_2_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_hand_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_2_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_tool_center_point:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Loop Animation: false
+ Name: Trajectory
+ Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
+ Robot Description: robot_description
+ Show Robot Collision: false
+ Show Robot Visual: true
+ Show Trail: false
+ State Display Time: 0.05 s
+ Trail Step Size: 1
+ Trajectory Topic: /display_planned_path
+ Use Sim Time: false
+ Value: true
+ - Attached Body Color: 150; 50; 150
+ Class: moveit_rviz_plugin/RobotState
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_1_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_1_tool_center_point:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_2_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_hand_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_2_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_2_tool_center_point:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Name: RobotState
+ Robot Alpha: 1
+ Robot Description: robot_description
+ Robot State Topic: /display_robot_state
+ Show All Links: true
+ Show Highlights: true
+ Value: true
+ Visual Enabled: true
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
diff --git a/results/dummy/-2642749.yaml b/results/dummy/-2642749.yaml
index cb62992cd4b5c4bd51f66897923e42b77d02c4b4..89be48e96023a389f31ea575add6333744ab1807 100644
--- a/results/dummy/-2642749.yaml
+++ b/results/dummy/-2642749.yaml
@@ -21,5 +21,12 @@
{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
-{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.104997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.104997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+ { 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+ { 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+ { 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
+ { 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+ { 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+ { 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ { 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }
]}
\ No newline at end of file
diff --git a/results/dummy/-562289182.yaml b/results/dummy/-562289182.yaml
index 3b44962e51121df05778c0bbe94e2dd2bdcb38fd..3e97a90bb8b87af55c962bce4357605093a705b5 100644
--- a/results/dummy/-562289182.yaml
+++ b/results/dummy/-562289182.yaml
@@ -21,5 +21,12 @@
{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } },
{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
-{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+ { 'id': 'blue1', 'type': 'BOX', 'pos': { 'x': .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+ { 'id': 'blue2', 'type': 'BOX', 'pos': { 'x': .2, 'y': .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+ { 'id': 'blue3', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
+ { 'id': 'green1', 'type': 'BOX', 'pos': { 'x': .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+ { 'id': 'green2', 'type': 'BOX', 'pos': { 'x': .1, 'y': 1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+ { 'id': 'red1', 'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+ { 'id': 'red2', 'type': 'BOX', 'pos': { 'x': .3, 'y': 1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } }
]}
\ No newline at end of file
diff --git a/src/cell_routine.cpp b/src/cell_routine.cpp
index 8cb09aafd295a1edf0c8abe8cb931e31601878e8..ea4be5c4476160dfb556129061137e95b1c61776 100644
--- a/src/cell_routine.cpp
+++ b/src/cell_routine.cpp
@@ -43,16 +43,13 @@ int main(int argc, char **argv){
Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator);
std::unique_ptr<Abstract_param_reader> robot_reader(new Robot_reader(nh));
- robot_reader->read();
auto rd = static_cast<Robot_reader*>(robot_reader.get())->robot_data();
for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
std::unique_ptr<Abstract_param_reader> wing_reader(new Wing_reader(nh));
- wing_reader->read();
auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data();
std::unique_ptr<Abstract_param_reader> cuboid_reader(new Cuboid_reader(nh));
- cuboid_reader->read();
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader.get())->cuboid_bin();
auto cod = static_cast<Cuboid_reader*>(cuboid_reader.get())->cuboid_obstacle();
@@ -79,6 +76,7 @@ int main(int argc, char **argv){
//moveit::planning_interface::PlanningSceneInterface::applyCollisionObject()
ceti1->notify();
ceti2->notify();
+
moveit_mediator->mediate();
diff --git a/src/impl/moveit_grasp_mediator.cpp b/src/impl/moveit_grasp_mediator.cpp
index 463b09f44fefd90e82864036cd1e7e2f51e48eb1..a933e6b2f0f34dcae5fab341bdfa1c58cf9cc9a2 100644
--- a/src/impl/moveit_grasp_mediator.cpp
+++ b/src/impl/moveit_grasp_mediator.cpp
@@ -1,5 +1,8 @@
#include "impl/moveit_grasp_mediator.h"
#include <regex>
+#include <gb_grasp/MapConfigLoader.h>
+#include <moveit/robot_state/conversions.h>
+
void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
robots_.push_back(robot);
@@ -7,29 +10,36 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
auto mr = dynamic_cast<Moveit_robot*>(robot);
manipulate_grasp_data(mr);
-
- ROS_INFO("1");
+ manipulate_mapspace();
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;
- ROS_INFO("2");
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("3");
map_gen->SetQuality(min_quality);
map_gen->SetMaxGrasp(max_candidates);
map_gen->SetZGraspOnly(top_only);
mr->set_grasp_map_generator(map_gen);
- ROS_INFO("4");
}
+void Moveit_grasp_mediator::manipulate_mapspace(){
+ std::vector<float> pos_voxel_space;
+ std::vector<float> pos_mapspace;
+ nh_->getParam("/voxel_space/pos", pos_voxel_space);
+ nh_->getParam("/mapspace/pos", pos_mapspace);
+ pos_mapspace[2]= 0.91f;
+ pos_voxel_space[2]= 0.91f;
+ nh_->setParam("/voxel_space/pos", pos_voxel_space);
+ nh_->setParam("/mapspace/pos", pos_mapspace);
+}
+
void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
XmlRpc::XmlRpcValue data;
nh_->getParam("/", data);
@@ -58,8 +68,14 @@ void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
std::stringstream ss;
ss << "panda_" << match[1] << "_tool_center_point";
- data[robot->map().at("eef_name").c_str()]["define_tcp_by_name"] = true;
+ 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;
@@ -83,7 +99,346 @@ void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
nh_->setParam("/", data);
}
-void Moveit_grasp_mediator::mediate(){}
+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.5;
+ 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();
+ /*
+ MapConfigLoader map_config(*nh_, grasp_ns_);
+ auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
+
+ map_config.SetObjects(cbd);
+ map_config.Create(grasp_maps_);
+ ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
+ {
+ ROS_INFO("creating map ...");
+ auto start_time = ros::Time::now();
+ for (auto* r : robots_){
+ auto mr = dynamic_cast<Moveit_robot*>(r);
+ for (GraspMap &m: grasp_maps_) { mr->grasp_map_generator()->SampleMap(m); }
+ double durration = (start_time - ros::Time::now()).toSec();
+ ROS_INFO_STREAM(grasp_maps_.size() << " GraspMaps created in " << durration << " seconds");
+ }
+ }
+
+
+ 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_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();
@@ -91,16 +446,71 @@ void Moveit_grasp_mediator::scene_setup(){
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);
- //planning_scene_interface_->addCollisionObjects({o.getCoMsg()});
+ //o.publish(visual_tools_,rviz_visual_tools::GREEN);
+ psi_->addCollisionObjects({o.getCoMsg()});
}
+
visual_tools_->publishRobotState(mgi_->getCurrentState());
visual_tools_->trigger();
-}
\ No newline at end of file
+}
+
+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)) {
+
+ 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_);
+
+ 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 = static_cast<Robot_reader*>(robot_reader_.get())->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();
+ };
\ No newline at end of file
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index c194e912138c47337edc9032ba431bfb29f9b16d..98fa329f2795583483bf9d34169cbbd70d775c4b 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -883,3 +883,36 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll
}
+Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
+ : Abstract_mediator(objects, pub), nh_(nh)
+ , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
+ , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
+ , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
+ , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
+ , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
+ , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description")){
+
+ if (planning_scene_monitor_->getPlanningScene()){
+ planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
+ "planning_scene");
+ planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
+ } else {
+ ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
+ return;
+ }
+
+ robot_model_loader::RobotModelLoaderPtr robot_model_loader;
+ robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
+ robot_model_ = robot_model_loader->getModel();
+ ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
+
+ // planner
+ sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
+
+ // cartesian
+ cartesian_planner_->setMaxVelocityScaling(1.0);
+ cartesian_planner_->setMaxAccelerationScaling(1.0);
+ cartesian_planner_->setStepSize(.01);
+
+ };
+
diff --git a/src/reader/cuboid_reader.cpp b/src/reader/cuboid_reader.cpp
index 8842cceb7fc4437ab8c4820d50f98cdf13d5fe15..e95a8c33652633346fd6b715e31a15cf3bdf7285 100644
--- a/src/reader/cuboid_reader.cpp
+++ b/src/reader/cuboid_reader.cpp
@@ -38,7 +38,7 @@ void Cuboid_reader::read(){
o.z_heigth = size.getZ();
- cuboid_bin_.push_back(o);
+ cuboid_box_.push_back(o);
ROS_INFO("--- Object: %s --- Type: BOX ---", o.Name.c_str());
ROS_INFO("=> Object: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
ROS_INFO("=> Object: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());