Hello,
I am trying to get a simple pick and place program to work. I modified [this](https://github.com/ros-planning/moveit_pr2/blob/groovy-devel/pr2_moveit_tutorials/pick_place/src/pick_place_tutorial.cpp) file to work with my MoveIt! package. The resulting code is shown below. When I try to run this code I get the following error:
It seems the planning goes well, but the moment the plan is executed this error appears. Also, when I simulate in RVIZ the 'planning model' of the robot seems to be moving without any problems. The 'real robot model' though, is not moving whatsoever. Which seems to correspond pretty well with the error message.
I've been stuck on this error for almost the entire day and have no clue where it comes from. Does anyone have an idea on what could be the problem?
Thank you for your help!
The code:
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Ioan Sucan */
#include
// MoveIt!
#include
#include
#include
//#include
static const std::string ROBOT_DESCRIPTION="robot_description";
void pick(moveit::planning_interface::MoveGroup &group)
{
std::vector grasps;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_link";
p.pose.position.x = 0.5;
p.pose.position.y = -0.7;
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;
moveit_msgs::Grasp g;
g.grasp_pose = p;
g.pre_grasp_approach.direction.vector.x = 1.0;
g.pre_grasp_approach.direction.header = p.header;
g.pre_grasp_approach.min_distance = 0.1;
g.pre_grasp_approach.desired_distance = 0.2;
g.post_grasp_retreat.direction.header = p.header;
g.post_grasp_retreat.direction.vector.z = 1.0;
g.post_grasp_retreat.min_distance = 0.1;
g.post_grasp_retreat.desired_distance = 0.25;
g.pre_grasp_posture.joint_names.resize(1, "joint_6");
g.pre_grasp_posture.points.resize(1);
g.pre_grasp_posture.points[0].positions.resize(1);
g.pre_grasp_posture.points[0].positions[0] = 1;
g.grasp_posture.joint_names.resize(1, "joint_6");
g.grasp_posture.points.resize(1);
g.grasp_posture.points[0].positions.resize(1);
g.grasp_posture.points[0].positions[0] = 0;
grasps.push_back(g);
group.setSupportSurfaceName("table");
ROS_INFO("Starting pick");
group.pick("part", grasps);
}
void place(moveit::planning_interface::MoveGroup &group)
{
std::vector loc;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_link";
p.pose.position.x = 0.0;
p.pose.position.y = 0.0;
p.pose.position.z = 0.0;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
moveit_msgs::PlaceLocation g;
g.place_pose = p;
g.pre_place_approach.direction.vector.z = -1.0;
g.post_place_retreat.direction.vector.x = -1.0;
g.post_place_retreat.direction.header.frame_id = "base_link";
g.pre_place_approach.direction.header.frame_id = "link_6";
g.pre_place_approach.min_distance = 0.1;
g.pre_place_approach.desired_distance = 0.2;
g.post_place_retreat.min_distance = 0.1;
g.post_place_retreat.desired_distance = 0.25;
g.post_place_posture.joint_names.resize(1, "joint_6");
g.post_place_posture.points.resize(1);
g.post_place_posture.points[0].positions.resize(1);
g.post_place_posture.points[0].positions[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 = "link_6";
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");
ROS_INFO("Starting place");
group.place("part", loc);
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "manipulator");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle nh;
ros::Publisher pub_co = nh.advertise("collision_object", 10);
ros::WallDuration(1.0).sleep();
moveit::planning_interface::MoveGroup group("manipulator");
group.setPlanningTime(45.0);
moveit_msgs::CollisionObject co;
co.header.stamp = ros::Time::now();
co.header.frame_id = "base_link";
// remove pole
co.id = "pole";
co.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co.publish(co);
// add pole
co.operation = moveit_msgs::CollisionObject::ADD;
co.primitives.resize(1);
co.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
co.primitives[0].dimensions.resize(shape_tools::SolidPrimitiveDimCount::value);
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.3;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 1.0;
co.primitive_poses.resize(1);
co.primitive_poses[0].position.x = 0.7;
co.primitive_poses[0].position.y = -0.4;
co.primitive_poses[0].position.z = 0.85;
co.primitive_poses[0].orientation.w = 1.0;
pub_co.publish(co);
// remove table
co.id = "table";
co.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co.publish(co);
// add table
co.operation = moveit_msgs::CollisionObject::ADD;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.5;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 1.5;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.35;
co.primitive_poses[0].position.x = 0.7;
co.primitive_poses[0].position.y = -0.2;
co.primitive_poses[0].position.z = 0.175;
pub_co.publish(co);
co.id = "part";
co.operation = moveit_msgs::CollisionObject::REMOVE;
pub_co.publish(co);
co.operation = moveit_msgs::CollisionObject::ADD;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] = 0.15;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] = 0.1;
co.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = 0.3;
co.primitive_poses[0].position.x = 0.6;
co.primitive_poses[0].position.y = -0.7;
co.primitive_poses[0].position.z = 0.5;
pub_co.publish(co);
// wait a bit for ros things to initialize
ros::WallDuration(1.0).sleep();
std::string s;
std::cin >> s;
pick(group);
std::cin >> s;
ros::WallDuration(1.0).sleep();
place(group);
ros::waitForShutdown();
return 0;
}
↧