Skip to content
Snippets Groups Projects
Commit 5dfa01c1 authored by KingMaZito's avatar KingMaZito
Browse files

behavioural analysis in ferdinand's package

parent 5ed8d517
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 870 additions and 119 deletions
include/impl/moveit_grasp_mediator.h
+ 20
− 47
View file @ 5dfa01c1
......@@ -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);
};
......
include/impl/moveit_mediator.h
+ 1
− 23
View file @ 5dfa01c1
......@@ -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_;};
......
include/impl/moveit_robot.h
+ 2
− 0
View file @ 5dfa01c1
......@@ -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;}
......
include/reader/cuboid_reader.h
+ 3
− 3
View file @ 5dfa01c1
......@@ -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;
......
launch/mtc_auerswald.launch
+ 43
− 30
View file @ 5dfa01c1
......@@ -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>
......
launch/rviz/cell_routine.rviz
+ 330
− 0
View file @ 5dfa01c1
......@@ -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
......
results/dummy/-2642749.yaml
+ 8
− 1
View file @ 5dfa01c1
......@@ -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
results/dummy/-562289182.yaml
+ 8
− 1
View file @ 5dfa01c1
......@@ -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
src/cell_routine.cpp
+ 1
− 3
View file @ 5dfa01c1
......@@ -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();
......
src/impl/moveit_grasp_mediator.cpp
+ 420
− 10
View file @ 5dfa01c1
#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();
}
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
src/impl/moveit_mediator.cpp
+ 33
− 0
View file @ 5dfa01c1
......@@ -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);
};
src/reader/cuboid_reader.cpp
+ 1
− 1
View file @ 5dfa01c1
......@@ -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());
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment