Skip to content
This repository has been archived by the owner on Nov 13, 2017. It is now read-only.

Commit

Permalink
added ignor of collision during movements
Browse files Browse the repository at this point in the history
  • Loading branch information
nlyubova committed Aug 21, 2015
1 parent faeccb1 commit 2507870
Show file tree
Hide file tree
Showing 15 changed files with 246 additions and 35 deletions.
107 changes: 94 additions & 13 deletions manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,7 @@ void move_group::MoveGroupPickPlaceAction::startPlaceLookCallback()

void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanOnly(const moveit_msgs::PickupGoalConstPtr& goal, moveit_msgs::PickupResult &action_res)
{
std::cout << "-- 0) move_group 96 " << std::endl;
pick_place::PickPlanPtr plan;
try
{
Expand Down Expand Up @@ -135,6 +136,7 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanOnly(const

void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const moveit_msgs::PlaceGoalConstPtr& goal, moveit_msgs::PlaceResult &action_res)
{
std::cout << "-- 0) move_group 139 " << std::endl;
pick_place::PlacePlanPtr plan;
try
{
Expand Down Expand Up @@ -178,6 +180,7 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallback_PlanOnly(const m
bool move_group::MoveGroupPickPlaceAction::planUsingPickPlace_Pickup(const moveit_msgs::PickupGoal& goal, moveit_msgs::PickupResult *action_res,
plan_execution::ExecutableMotionPlan &plan)
{
std::cout << "-- 0) move_group 183 " << std::endl;
setPickupState(PLANNING);

planning_scene_monitor::LockedPlanningSceneRO ps(plan.planning_scene_monitor_);
Expand Down Expand Up @@ -269,6 +272,9 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback_PlanAndExecute(
{
plan_execution::PlanExecution::Options opt;

std::cout << "-- 0) move_group 275 goal->planning_options.replan_attempts =" << goal->planning_options.replan_attempts << std::endl;
std::cout << "-- 0) move_group 276 goal->planning_options.replan_delay =" << goal->planning_options.replan_delay << std::endl;

opt.replan_ = goal->planning_options.replan;
opt.replan_attempts_ = goal->planning_options.replan_attempts;
opt.replan_delay_ = goal->planning_options.replan_delay;
Expand Down Expand Up @@ -325,14 +331,19 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallback(const moveit_ms
context_->planning_scene_monitor_->updateFrameTransforms();

moveit_msgs::PickupGoalConstPtr goal;
std::cout << "-- 0) move_group 328 input_goal->possible_grasps.empty()=" << input_goal->possible_grasps.empty() << std::endl;
if (input_goal->possible_grasps.empty())
{
std::cout << "-- 0) move_group 336 empty " << std::endl;
moveit_msgs::PickupGoal* copy(new moveit_msgs::PickupGoal(*input_goal));
goal.reset(copy);
fillGrasps(*copy);
}
else
{
std::cout << "-- 0) move_group 343 non-empty" << std::endl;
goal = input_goal;
}

moveit_msgs::PickupResult action_res;

Expand Down Expand Up @@ -427,6 +438,7 @@ void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& g
collision_detection::World::ObjectConstPtr object = lscene->getWorld()->getObject(goal.target_name);
if (object && !object->shape_poses_.empty())
{
//std::cout << "qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq "<< object->shape_poses_[0].pose << std::endl;
request.arm_name = goal.group_name;
request.target.reference_frame_id = lscene->getPlanningFrame();

Expand Down Expand Up @@ -456,9 +468,12 @@ void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& g
if (valid)
{
ROS_DEBUG_NAMED("manipulation", "Calling grasp planner...");
std::cout << "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
std::cout << "-- *) move_group 469 << grasp_planning_service_.call(request, response)=" << grasp_planning_service_.call(request, response) << std::endl;
if (grasp_planning_service_.call(request, response))
{
goal.possible_grasps.resize(response.grasps.size());
std::cout << "-- *) move_group 474 response.grasps.size()=" << response.grasps.size() << std::endl;
for (std::size_t i = 0 ; i < response.grasps.size() ; ++i)
{
// construct a moveit grasp from a grasp planner grasp
Expand Down Expand Up @@ -507,42 +522,108 @@ void move_group::MoveGroupPickPlaceAction::fillGrasps(moveit_msgs::PickupGoal& g

if (goal.possible_grasps.empty())
{
//figure out the end-effector
if (goal.end_effector.empty() && !goal.group_name.empty())
{
const robot_model::JointModelGroup *jmg = lscene->getRobotModel()->getJointModelGroup(goal.group_name);
if (!jmg)
ROS_WARN_STREAM_NAMED("move_group", "Invalid group name '" << moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME << "'");
else
{
const std::vector<std::string> &eefs = jmg->getAttachedEndEffectorNames();
if (!eefs.empty())
goal.end_effector = eefs.front();
}
}
std::cout << "** *) move_group 550 goal.group_name=" << goal.group_name << ", goal.end_effector=" << goal.end_effector << std::endl;
ROS_DEBUG_NAMED("manipulation", "Using default grasp poses");
goal.minimize_object_distance = true;
//goal.minimize_object_distance = true;

// add a number of default grasp points
// \todo add more!
moveit_msgs::Grasp g;
g.grasp_pose.header.frame_id = goal.target_name;
g.grasp_pose.pose.position.x = -0.2;
g.grasp_pose.pose.position.x = 0.07;
g.grasp_pose.pose.position.y = 0.0;
g.grasp_pose.pose.position.z = 0.0;
g.grasp_pose.pose.position.z = 0.125;
g.grasp_pose.pose.orientation.x = 0.0;
g.grasp_pose.pose.orientation.y = 0.0;
g.grasp_pose.pose.orientation.z = 0.0;
g.grasp_pose.pose.orientation.w = 1.0;
g.grasp_pose.header.stamp = ros::Time::now();
std::cout << " 99999999999999999999999999 goal.target_name=" << goal.target_name << std::endl;
collision_detection::World::ObjectConstPtr object = lscene->getWorld()->getObject(goal.target_name);
if (object && !object->shape_poses_.empty())
{
tf::poseEigenToMsg(object->shape_poses_[0], g.grasp_pose.pose);
}

g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame();
g.pre_grasp_approach.direction.vector.x = 1.0;
g.pre_grasp_approach.min_distance = 0.1;
g.pre_grasp_approach.desired_distance = 0.2;
// data for generating grasps
/*moveit_simple_grasps::GraspData grasp_data_;
if (!grasp_data_.loadRobotGraspData(root_node_handle_, end_effector_))
std::vector<moveit_msgs::Grasp> grasps;
simple_grasps_->generateBlockGrasps( block_pose, grasp_data_, goal.possible_grasps );*/

g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame();
g.post_grasp_retreat.direction.vector.z = 1.0;
g.post_grasp_retreat.min_distance = 0.1;
g.pre_grasp_approach.direction.header.frame_id = lscene->getPlanningFrame(); //"LWristYaw_link";
g.pre_grasp_approach.direction.vector.x = 0;
g.pre_grasp_approach.direction.vector.y = 0;
g.pre_grasp_approach.direction.vector.z = -1;
g.pre_grasp_approach.min_distance = 0.06;
g.pre_grasp_approach.desired_distance = 0.2;
g.pre_grasp_approach.direction.header.stamp = ros::Time::now();
std::cout << " 99999999999999999999999999 lscene->getPlanningFrame()=" << lscene->getPlanningFrame() << std::endl;

g.post_grasp_retreat.direction.header.frame_id = lscene->getPlanningFrame(); //"LWristYaw_link";
g.post_grasp_retreat.direction.vector.x = 0;
g.post_grasp_retreat.direction.vector.y = 0;
g.post_grasp_retreat.direction.vector.z = 1; // Retreat direction (pos z axis)
g.post_grasp_retreat.min_distance = 0.06;
g.post_grasp_retreat.desired_distance = 0.2;
g.post_grasp_retreat.direction.header.stamp = ros::Time::now();

if (lscene->getRobotModel()->hasEndEffector(goal.end_effector))
{
g.pre_grasp_posture.joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();
//g.pre_grasp_posture.joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();

std::cout << "9999999999999 getJointModelNames().size() == 2? " << lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames().size() << std::endl;

//define the pre-grasp posture
g.pre_grasp_posture.joint_names.resize(1);
std::vector<std::string> joint_names = lscene->getRobotModel()->getEndEffector(goal.end_effector)->getJointModelNames();
for (std::vector<std::string>::iterator joint_name_i = joint_names.begin(); joint_name_i != joint_names.end(); ++joint_name_i)
if (*joint_name_i == "LHand")
g.pre_grasp_posture.joint_names[0] = "LHand";
else if (*joint_name_i == "RHand")
g.pre_grasp_posture.joint_names[0] = "RHand";

g.pre_grasp_posture.header.stamp = ros::Time::now();
g.pre_grasp_posture.header.frame_id = lscene->getPlanningFrame(); //"base_link";
g.pre_grasp_posture.points.resize(1);
g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(), std::numeric_limits<double>::max());
g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size());
//g.pre_grasp_posture.points[0].positions.resize(g.pre_grasp_posture.joint_names.size(), std::numeric_limits<double>::max());
g.pre_grasp_posture.points[0].positions[0] = 1.0;

//define the grasp posture
g.grasp_posture.joint_names = g.pre_grasp_posture.joint_names;
//std::cout << "** *) move_group 609 g.grasp_posture.joint_names.size()" << g.grasp_posture.joint_names.size() << " " << g.grasp_posture.joint_names[0] << std::endl;
g.grasp_posture.header.stamp = ros::Time::now();
g.grasp_posture.header.frame_id = lscene->getPlanningFrame(); //"base_link";
g.grasp_posture.points.resize(1);
g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), -std::numeric_limits<double>::max());
g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), 0.0);
//g.grasp_posture.points[0].positions.resize(g.grasp_posture.joint_names.size(), -std::numeric_limits<double>::max());
g.grasp_posture.points[0].positions[0] = 0.0;
}

std::cout << "** *) move_group 618 goal.target_name= " << goal.target_name << std::endl; //TODO chekc if non-empty
std::vector<std::string> allowed_touch_objects(1);
allowed_touch_objects[0] = goal.target_name;
g.allowed_touch_objects = allowed_touch_objects;
goal.possible_grasps.push_back(g);

std::cout << "9999999999999999999999999999999999999 --grasp start" << std::endl;
std::cout << g << std::endl;
std::cout << "9999999999999999999999999999999999999 --grasp end" << std::endl;
}
}

Expand Down
28 changes: 26 additions & 2 deletions manipulation/pick_place/src/approach_and_translate_stage.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,10 +49,16 @@ ApproachAndTranslateStage::ApproachAndTranslateStage(const planning_scene::Plann
planning_scene_(scene),
collision_matrix_(collision_matrix)
{
std::cout << "////////////////////// stage 2 is approach & translate" << std::endl;
max_goal_count_ = GetGlobalPickPlaceParams().max_goal_count_;
max_fail_ = GetGlobalPickPlaceParams().max_fail_;
max_step_ = GetGlobalPickPlaceParams().max_step_;
jump_factor_ = GetGlobalPickPlaceParams().jump_factor_;
std::cout << "-- ApproachAndTranslateStage 57 "
<< " max_goal_count_=" << max_goal_count_
<< " max_fail_=" << max_fail_
<< " max_step_ =" << max_step_
<< " jump_factor_ =" << jump_factor_ << std::endl;
}

namespace
Expand All @@ -66,6 +72,7 @@ bool isStateCollisionFree(const planning_scene::PlanningScene *planning_scene,
const robot_state::JointModelGroup *group,
const double *joint_group_variable_values)
{
std::cout << "---- stage 2 ApproachAndTranslateStage isStateCollisionFree" << std::endl;
state->setJointGroupPositions(group, joint_group_variable_values);

collision_detection::CollisionRequest req;
Expand All @@ -80,6 +87,7 @@ bool isStateCollisionFree(const planning_scene::PlanningScene *planning_scene,
state->setVariablePositions(grasp_posture->joint_names, grasp_posture->points[i].positions);
collision_detection::CollisionResult res;
planning_scene->checkCollision(req, res, *state, *collision_matrix);
//std::cout << "----isStateCollisionFree 90 res.collision =" << res.collision << std::endl;
if (res.collision)
return false;
}
Expand All @@ -88,6 +96,7 @@ bool isStateCollisionFree(const planning_scene::PlanningScene *planning_scene,
{
collision_detection::CollisionResult res;
planning_scene->checkCollision(req, res, *state, *collision_matrix);
//std::cout << "----isStateCollisionFree 99 res.collision =" << res.collision << std::endl;
if (res.collision)
return false;
}
Expand All @@ -97,6 +106,7 @@ bool isStateCollisionFree(const planning_scene::PlanningScene *planning_scene,
bool samplePossibleGoalStates(const ManipulationPlanPtr &plan, const robot_state::RobotState &reference_state,
double min_distance, unsigned int attempts)
{
std::cout << "---- stage 2 ApproachAndTranslateStage samplePossibleGoalStates" << std::endl;
// initialize with scene state
robot_state::RobotStatePtr token_state(new robot_state::RobotState(reference_state));
for (unsigned int j = 0 ; j < attempts ; ++j)
Expand Down Expand Up @@ -128,6 +138,7 @@ bool executeAttachObject(const ManipulationPlanSharedDataConstPtr &shared_plan_d
const trajectory_msgs::JointTrajectory &detach_posture,
const plan_execution::ExecutableMotionPlan *motion_plan)
{
std::cout << "---- stage 2 ApproachAndTranslateStage executeAttachObject" << std::endl;
ROS_DEBUG_NAMED("manipulation", "Applying attached object diff to maintained planning scene (attaching/detaching object to end effector)");
bool ok = false;
{
Expand All @@ -146,6 +157,7 @@ bool executeAttachObject(const ManipulationPlanSharedDataConstPtr &shared_plan_d
// Add the close end effector trajectory to the overall plan (after the approach trajectory, before the retreat)
void addGripperTrajectory(const ManipulationPlanPtr &plan, const collision_detection::AllowedCollisionMatrixConstPtr &collision_matrix, const std::string &name)
{
std::cout << "---- stage 2 ApproachAndTranslateStage 158 addGripperTrajectory" << std::endl;
// Check if a "closed" end effector configuration was specified
if (!plan->retreat_posture_.joint_names.empty())
{
Expand Down Expand Up @@ -180,6 +192,7 @@ void addGripperTrajectory(const ManipulationPlanPtr &plan, const collision_detec

bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
{
std::cout << "////////////////////// stage 2 ApproachAndTranslateStage::evaluate" << std::endl;
const robot_model::JointModelGroup *jmg = plan->shared_data_->planning_group_;
// compute what the maximum distance reported between any two states in the planning group could be, and keep 1% of that;
// this is the minimum distance between sampled goal states
Expand All @@ -194,6 +207,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
bool approach_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->approach_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());
bool retreat_direction_is_global_frame = !robot_state::Transforms::sameFrame(plan->retreat_.direction.header.frame_id, plan->shared_data_->ik_link_->getName());

std::cout << "---- stage 2 ApproachAndTranslateStage::evaluate approach_direction_is_global_frame" << std::endl;
// transform the input vectors in accordance to frame specified in the header;
if (approach_direction_is_global_frame)
approach_direction = planning_scene_->getFrameTransform(plan->approach_.direction.header.frame_id).rotation() * approach_direction;
Expand All @@ -203,10 +217,13 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
// state validity checking during the approach must ensure that the gripper posture is that for pre-grasping
robot_state::GroupStateValidityCallbackFn approach_validCallback = boost::bind(&isStateCollisionFree, planning_scene_.get(),
collision_matrix_.get(), verbose_, &plan->approach_posture_, _1, _2, _3);
std::cout << "---- stage 2 ApproachAndTranslateStage::evaluate 213 max_goal_count_" << plan->possible_goal_states_.size() << "==" << max_goal_count_ << std::endl;

plan->goal_sampler_->setVerbose(verbose_);
std::size_t attempted_possible_goal_states = 0;
do // continously sample possible goal states
{
std::cout << "---- stage 2 ApproachAndTranslateStage::evaluate 219 max_goal_count_=" << plan->possible_goal_states_.size() << "==" << max_goal_count_ << " -> " << signal_stop_ << std::endl;
for (std::size_t i = attempted_possible_goal_states ; i < plan->possible_goal_states_.size() && !signal_stop_ ; ++i, ++attempted_possible_goal_states)
{
// if we are trying to get as close as possible to the goal (maximum one meter)
Expand All @@ -233,9 +250,11 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
-approach_direction, approach_direction_is_global_frame, plan->approach_.desired_distance,
max_step_, jump_factor_, approach_validCallback);

std::cout << " -------- 246 " << d_approach << ">" << plan->approach_.min_distance << "&&" << !signal_stop_ << std::endl;
// if we were able to follow the approach direction for sufficient length, try to compute a retreat direction
if (d_approach > plan->approach_.min_distance && !signal_stop_)
if (d_approach > plan->approach_.min_distance/2.0 && !signal_stop_)
{
std::cout << " -------- 250 " << plan->retreat_.desired_distance << std::endl;
if (plan->retreat_.desired_distance > 0.0)
{
// construct a planning scene that is just a diff on top of our actual planning scene
Expand All @@ -259,6 +278,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
max_step_, jump_factor_, retreat_validCallback);

// if sufficient progress was made in the desired direction, we have a goal state that we can consider for future stages
std::cout << " -------- 275 " << d_retreat << " > " << plan->retreat_.min_distance << " && " << !signal_stop_ << std::endl;
if (d_retreat > plan->retreat_.min_distance && !signal_stop_)
{
// Create approach trajectory
Expand Down Expand Up @@ -324,7 +344,11 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr &plan) const
}
}
while (plan->possible_goal_states_.size() < max_goal_count_ && !signal_stop_ && samplePossibleGoalStates(plan, planning_scene_->getCurrentState(), min_distance, max_fail_));

std::cout << "---------------- stage 2 ApproachAndTranslateStage::evaluate 335 "
<< " max_goal_count_=" << max_goal_count_ << std::endl
<< "min_distance= " << min_distance << std::endl
<< "max_fail_=" << max_fail_ << std::endl
<< "plan->possible_goal_states_.size() =" << plan->possible_goal_states_.size() << std::endl;
plan->error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;

return false;
Expand Down
Loading

0 comments on commit 2507870

Please sign in to comment.