I'm trying to grasp an object on a table, attach it to the hand, and drop it off somewhere else with the Jaco2 robotic arm.
Grasping works fine: I attach the object and close the grippers, which leads me to believe the collision is (correctly!) ignored for gripping the object, but as soon as I want to drop it, I often get the following error.
[INFO] Found a contact between 'cup' [Robot attached] and 'jaco_finger_tip_1' [Robot link]
[WARN] Start State seems to be in collision with respect to group arm.
This happens in 1 out of 5 times, I would say. The Planner I use is RTTConnectkDefaultConfig.
I am sure, that the object is attached, for multiple reasons:
a. I have a fixed tabletop already in the planning scene. If i don't lower it a cm or so lower than it actually is, I get this error:
` [INFO] Found a contact between 'cup' [Robot attached] and 'jaco_finger_tip_1' [Robot link] `
b. I check again and again with the move_group interface, if object exists and if it is attached. I get no error messages here
auto all_objects = planning_scene_interface_.getObjects();
if (all_objects.find(box.description) == all_objects.end()) {
ROS_ERROR_STREAM("Object " << box.description << " hasn't been created");
return;
}
// moveit_visuals_.attachObstacle(box);
auto all_attached_objects = planning_scene_interface_.getAttachedObjects();
if (all_attached_objects.find(box.description) != all_attached_objects.end()) {
ROS_WARN_STREAM("Object " << box.description << " is already attached");
return;
}
ROS_STATUS("Attaching obstacle " << box.description);
move_group_.attachObject(box.description);
Adding objects to the planning scene works fine:
const auto& pose_goal = goal->pose_goal;
const auto& box = goal->bounding_box;
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group_.getPlanningFrame();
collision_object.id = box.description;
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = box.dimensions.x;
primitive.dimensions[1] = box.dimensions.y;
primitive.dimensions[2] = box.dimensions.z;
geometry_msgs::PointStamped out_pt;
geometry_msgs::PointStamped in_pt;
in_pt.header = box.header;
in_pt.point = box.point;
// transform point
try {
tf_listener_.waitForTransform("root", box.header.frame_id, box.header.stamp, ros::Duration(1));
tf_listener_.transformPoint("root", in_pt, out_pt);
}
catch (tf::TransformException &exception) {
ROS_INFO_STREAM("Transform failed. Why? - " << exception.what());
}
geometry_msgs::Pose pose;
pose.position.x = out_pt.point.x;
pose.position.y = out_pt.point.y;
pose.position.z = out_pt.point.z;
pose.orientation.w = 1.0;
collision_object.operation = collision_object.ADD;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(pose);
std::vector collision_objects;
collision_objects.push_back(collision_object);
planning_scene_interface_.addCollisionObjects(collision_objects);
ROS_STATUS("Added Object " << box.description);
detaching/attaching in accordance with tutorials
void JacoManipulationServer::attachObstacle(const jaco_manipulation::BoundingBox &box) {
ROS_STATUS("Attaching obstacle " << box.description);
move_group_.attachObject(box.description);
}
void JacoManipulationServer::detachObstacle(const jaco_manipulation::BoundingBox &box) {
ROS_STATUS("Detaching obstacle " << box.description);
move_group_.detachObject(box.description);
}
this is my gripping code
addObstacle(goal);
bool moved = planAndMove(goal->pose_goal);
if (!moved) return false;
attachObstacle(goal->bounding_box);
closeGripper();
ROS_STATUS("Gripper closed. Object grasped.");
return true;
where i add the obstacle (code above), let moveit do planning and moeving towards goal, attach the obstacle, so it can be gripped, and close the gripper.
Similarly, for dropping
bool moved = planAndMove(goal->pose_goal);
if (!moved) {
openGripper();
detachObstacle(goal->bounding_box);
return false;
}
openGripper();
detachObstacle(goal->bounding_box);
ROS_STATUS("Gripper opened. Object dropped.");
return true;
I plan and move with moveit, open the grippers (and then detach the object, not the other way around, so opening the gripper on the detached object doesn't show collision)
Thanks a lot for the help
↧