void demoPlace(move_group_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::PlaceLocation> loc;
for (std::size_t i = 0 ; i < 20 ; ++i)
{
geometry_msgs::PoseStamped p = group.getRandomPose();
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::PlaceLocation g;
g.place_pose = p;
g.approach.direction.vector.x = 1.0;
g.retreat.direction.vector.z = 1.0;
g.retreat.direction.header = p.header;
g.approach.min_distance = 0.2;
g.approach.desired_distance = 0.4;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.27;
g.post_place_posture.name.resize(1, "r_gripper_joint");
g.post_place_posture.position.resize(1);
g.post_place_posture.position[0] = 0;
loc.push_back(g);
}
group.place("bubu", loc);
}
void place(move_group_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::PlaceLocation> loc;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_footprint";
p.pose.position.x = 0.42;
p.pose.position.y = 0.0;
p.pose.position.z = 0.5;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::PlaceLocation g;
g.place_pose = p;
g.approach.direction.vector.z = -1.0;
g.retreat.direction.vector.x = -1.0;
g.retreat.direction.header.frame_id = "base_footprint";
g.approach.direction.header.frame_id = "r_wrist_roll_link";
g.approach.min_distance = 0.1;
g.approach.desired_distance = 0.2;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.post_place_posture.name.resize(1, "r_gripper_joint");
g.post_place_posture.position.resize(1);
g.post_place_posture.position[0] = 1;
loc.push_back(g);
group.setSupportSurfaceName("table");
// add path constraints
moveit_msgs::Constraints constr;
constr.orientation_constraints.resize(1);
moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = p.header.frame_id;
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.2;
ocm.absolute_z_axis_tolerance = M_PI;
ocm.weight = 1.0;
group.setPathConstraints(constr);
group.setPlannerId("RRTConnectkConfigDefault");
group.place("part", loc);
}
bool place() {
//group->place(object_to_manipulate, generated_pickup_grasp.grasp_pose);
std::vector<manipulation_msgs::PlaceLocation> loc;
geometry_msgs::PoseStamped p;
/*
p.header.frame_id = BASE_LINK;
p.pose.position = object_to_manipulate_position.point;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
*/
p = generated_pickup_grasp.grasp_pose;
make_pose_reachable_by_5DOF_katana(p);
ROS_DEBUG_STREAM("Place Pose: " << p.pose);
manipulation_msgs::PlaceLocation g;
g.place_pose = p;
g.approach.direction.vector.z = -1.0;
g.retreat.direction.vector.x = -1.0;
g.retreat.direction.header.frame_id = BASE_LINK;
g.approach.direction.header.frame_id = GRIPPER_FRAME;
g.approach.min_distance = 0.1;
g.approach.desired_distance = 0.2;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.post_place_posture.name.resize(1, FINGER_JOINT);
g.post_place_posture.position.resize(1);
g.post_place_posture.position[0] = 0.30;
loc.push_back(g);
//group->setSupportSurfaceName("table");
/* Option path constraints (e.g. to always stay upright)
// add path constraints
moveit_msgs::Constraints constr;
constr.orientation_constraints.resize(1);
moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = p.header.frame_id;
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.2;
ocm.absolute_z_axis_tolerance = M_PI;
ocm.weight = 1.0;
group->setPathConstraints(constr);
group->setPlannerId("RRTConnectkConfigDefault");
*/
group->place(object_to_manipulate, loc);
/*
//create a place location
//geometry_msgs::PoseStamped place_location = pickup_location;
geometry_msgs::PoseStamped place_location;
place_location.header.stamp = ros::Time::now();
// ----- put the object down
ROS_INFO("Calling the place action");
object_manipulation_msgs::PlaceGoal place_goal;
place_location.header.frame_id = KATANA_BASE_LINK;
place_location.pose.position.x =
normalPoseRobotFrame.pose.position.x;
place_location.pose.position.y =
normalPoseRobotFrame.pose.position.y;
place_location.pose.position.z =
pickup_result.grasp.grasp_pose.position.z; //TODO: to calculate z object higth needs to be considered
tf::Transform position;
tf::poseMsgToTF(place_location.pose, position);
tf::Vector3 shift_direction;
shift_direction.setX(place_location.pose.position.x);
shift_direction.setY(place_location.pose.position.y);
shift_direction.setZ(0);
shift_direction.normalize();
tf::Vector3 shift_direction_inverse;
shift_direction_inverse.setX(-shift_direction.getX());
shift_direction_inverse.setY(-shift_direction.getY());
shift_direction_inverse.setZ(-shift_direction.getZ());
//transform place_position to the start of the test line
tf::Transform trans(tf::Quaternion(0, 0, 0, 1.0),
shift_direction_inverse * place_position_tolerance_in_meter);
position = trans * position;
//move along the test line and add positions to place_goal.place_locations
for (float offset = 0; offset <= place_position_tolerance_in_meter;
offset += place_planner_step_size_in_meter) {
tf::Transform trans(tf::Quaternion(0, 0, 0, 1.0),
shift_direction * place_planner_step_size_in_meter);
position = trans * position;
geometry_msgs::Pose position_pose;
tf::poseTFToMsg(position, position_pose);
place_location.pose = position_pose;
// Convert quaternion to RPY.
tf::Quaternion q;
double roll, pitch, yaw;
tf::quaternionMsgToTF(pickup_result.grasp.grasp_pose.orientation,
q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
//determine yaw which is compatible with the Katana 300 180 kinematics.
yaw = atan2(place_location.pose.position.y,
place_location.pose.position.x);
tf::Quaternion quat = tf::createQuaternionFromRPY(0.0, pitch, yaw);
place_location.pose.orientation.x = quat.getX();
place_location.pose.orientation.y = quat.getY();
place_location.pose.orientation.z = quat.getZ();
place_location.pose.orientation.w = quat.getW();
ROS_INFO(
"Place Location: XYZ, Quaternions xyzw = %lf,%lf,%lf, (%lf, %lf, %lf, %lf)", place_location.pose.position.x, place_location.pose.position.y, place_location.pose.position.z, place_location.pose.orientation.x, place_location.pose.orientation.y, place_location.pose.orientation.z, place_location.pose.orientation.w);
place_goal.place_locations.push_back(place_location);
}
//end place planner
//the collision names of both the objects and the table
//same as in the pickup action
place_goal.collision_object_name =
processing_call.response.collision_object_names.at(
object_to_pick_ind);
place_goal.collision_support_surface_name =
processing_call.response.collision_support_surface_name;
//set grasp orientation to identity (all info is already given by place_location)
place_goal.grasp.grasp_pose.orientation.x = 0;
place_goal.grasp.grasp_pose.orientation.y = 0;
place_goal.grasp.grasp_pose.orientation.z = 0;
place_goal.grasp.grasp_pose.orientation.w = 1;
place_goal.grasp.grasp_pose.position.x = 0;
place_goal.grasp.grasp_pose.position.y = 0;
place_goal.grasp.grasp_pose.position.z = 0;
//use the arm to place
place_goal.arm_name = "arm";
//padding used when determining if the requested place location
//would bring the object in collision with the environment
place_goal.place_padding = 0.02;
//how much the gripper should retreat after placing the object
place_goal.desired_retreat_distance = 0.06;
place_goal.min_retreat_distance = 0.05;
//we will be putting down the object along the "vertical" direction
//which is along the z axis in the base_link frame
geometry_msgs::Vector3Stamped direction;
direction.header.stamp = ros::Time::now();
direction.header.frame_id = KATANA_BASE_LINK;
direction.vector.x = 0;
direction.vector.y = 0;
direction.vector.z = -1;
place_goal.approach.direction = direction;
//request a vertical put down motion of 10cm before placing the object
place_goal.approach.desired_distance = 0.06;
place_goal.approach.min_distance = 0.05;
//we are not using tactile based placing
place_goal.use_reactive_place = false;
//send the goal
place_client->sendGoal(place_goal);
while (!place_client->waitForResult(ros::Duration(10.0))) {
ROS_INFO("Waiting for the place action...");
if (!nh.ok())
return -1;
}
object_manipulation_msgs::PlaceResult place_result =
*(place_client->getResult());
if (place_client->getState()
!= actionlib::SimpleClientGoalState::SUCCEEDED) {
if (place_result.manipulation_result.value
== object_manipulation_msgs::ManipulationResult::RETREAT_FAILED) {
ROS_WARN(
"Place failed with error RETREAT_FAILED, ignoring! This may lead to collision with the object we just placed!");
} else {
ROS_ERROR(
"Place failed with error code %d", place_result.manipulation_result.value);
return -1;
}
}
//success!
ROS_INFO("Success! Object moved.");
move_arm_out_of_the_way();
object_in_gripper = 0;
*/
return true;
}
