search

moveit / moveit_tutorials

A sphinx-based centralized documentation repo for MoveIt
https://moveit.github.io/moveit_tutorials/
BSD 3-Clause "New" or "Revised" License
472 stars 691 forks source link

Pick and Place Pipeline crash when Build Type is Debug #795

Closed rodpc07 closed 1 year ago

rodpc07 commented 1 year ago

Description

Hi, I am using a ABB Yumi and a couple of weeks ago I was able to successfully execute the pick and place tutorial with this arm. Today I tried running the code again but it crashes as soon as it calls the pick() function from the move_group. The only thing I did since the day it works correctly was run the setup assistant and generate the package again but I didn't change anything in the configuration of the setup assistant. I leave bellow the backtrace and thank in advance to anyone who knows what might have happen.

Your environment

Code

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_interface/planning_interface.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

#include <visualization_msgs/Marker.h>
#include <interactive_markers/interactive_marker_server.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <iostream>
#include <string>

#include <memory>

using namespace std;

void pick(moveit::planning_interface::MoveGroupInterface &arm_mgi, moveit::planning_interface::MoveGroupInterface &gripper_mgi)
{
    // BEGIN_SUB_TUTORIAL pick1
    // Create a vector of grasps to be attempted, currently only creating single grasp.
    // This is essentially useful when using a grasp generator to generate and test multiple grasps.
    std::vector<moveit_msgs::Grasp> grasps;
    grasps.resize(1);

    // Actual Grasping Pose
    grasps[0].grasp_pose.header.frame_id = "yumi_base_link";
    grasps[0].grasp_pose.pose.position.x = 0.383890;
    grasps[0].grasp_pose.pose.position.y = -0.008552;
    grasps[0].grasp_pose.pose.position.z = 0.491650;
    grasps[0].grasp_pose.pose.orientation.x = 0.471129;
    grasps[0].grasp_pose.pose.orientation.y = 0.538489;
    grasps[0].grasp_pose.pose.orientation.z = 0.500098;
    grasps[0].grasp_pose.pose.orientation.w = 0.487822;

    // Pre Grasp (vector representing the direction of approach)
    grasps[0].pre_grasp_approach.direction.header.frame_id = "yumi_base_link";
    grasps[0].pre_grasp_approach.direction.vector.x = 1.0;
    grasps[0].pre_grasp_approach.min_distance = 0.095;
    grasps[0].pre_grasp_approach.desired_distance = 0.115;

    // Post Grasp (vector representing the direction of retreat)
    grasps[0].post_grasp_retreat.direction.header.frame_id = "yumi_base_link";
    grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
    grasps[0].post_grasp_retreat.min_distance = 0.1;
    grasps[0].post_grasp_retreat.desired_distance = 0.25;

    // Open Gripper Pose
    for (const std::string joint_name : gripper_mgi.getVariableNames())
    {
        grasps[0].pre_grasp_posture.joint_names.push_back(joint_name);
    };

    grasps[0].pre_grasp_posture.points.resize(1);
    grasps[0].pre_grasp_posture.points[0].positions.resize(2);
    grasps[0].pre_grasp_posture.points[0].positions[0] = 0.025;
    grasps[0].pre_grasp_posture.points[0].positions[1] = 0.025;
    grasps[0].pre_grasp_posture.points[0].time_from_start = ros::Duration(0.5);

    // Close Gripper Pose
    for (const std::string joint_name : gripper_mgi.getVariableNames())
    {
        grasps[0].grasp_posture.joint_names.push_back(joint_name);
    };

    grasps[0].grasp_posture.points.resize(1);
    grasps[0].grasp_posture.points[0].positions.resize(2);
    grasps[0].grasp_posture.points[0].positions[0] = 0.00;
    grasps[0].grasp_posture.points[0].positions[1] = 0.00;
    grasps[0].grasp_posture.points[0].time_from_start = ros::Duration(0.5);

    arm_mgi.setSupportSurfaceName("table1");
    arm_mgi.pick("object", grasps);
}

void place(moveit::planning_interface::MoveGroupInterface &arm_mgi, moveit::planning_interface::MoveGroupInterface &gripper_mgi)
{
    std::vector<moveit_msgs::PlaceLocation> place_location;
    place_location.resize(1);

    // Actual Placing Pose (this is the object pose and not the gripper pose)
    place_location[0].place_pose.header.frame_id = "yumi_base_link";
    place_location[0].place_pose.pose.position.x = 0.02;
    place_location[0].place_pose.pose.position.y = 0.46;
    place_location[0].place_pose.pose.position.z = 0.5;
    place_location[0].place_pose.pose.orientation.x = 0;
    place_location[0].place_pose.pose.orientation.y = 0;
    place_location[0].place_pose.pose.orientation.z = 0.720831;
    place_location[0].place_pose.pose.orientation.w = 0.693111;

    // Pre Place Pose (vector representing the direction of approach, simetric to the pick)
    place_location[0].pre_place_approach.direction.header.frame_id = "yumi_base_link";
    place_location[0].pre_place_approach.direction.vector.z = -1.0;
    place_location[0].pre_place_approach.min_distance = 0.095;
    place_location[0].pre_place_approach.desired_distance = 0.115;

    // Pre Place Pose (vector representing the direction of retreat)
    place_location[0].post_place_retreat.direction.header.frame_id = "yumi_base_link";
    place_location[0].post_place_retreat.direction.vector.z = 1.0;
    place_location[0].post_place_retreat.min_distance = 0.1;
    place_location[0].post_place_retreat.desired_distance = 0.25;

    // Open Gripper Pose
    for (const std::string joint_name : gripper_mgi.getVariableNames())
    {
        place_location[0].post_place_posture.joint_names.push_back(joint_name);
    };

    place_location[0].post_place_posture.points.resize(1);
    place_location[0].post_place_posture.points[0].positions.resize(2);
    place_location[0].post_place_posture.points[0].positions[0] = 0.025;
    place_location[0].post_place_posture.points[0].positions[1] = 0.025;
    place_location[0].post_place_posture.points[0].time_from_start = ros::Duration(0.5);

    arm_mgi.setSupportSurfaceName("table2");
    arm_mgi.place("object", place_location);
}

void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface &planning_scene_interface)
{
    // BEGIN_SUB_TUTORIAL table1
    //
    // Creating Environment
    // ^^^^^^^^^^^^^^^^^^^^
    // Create vector to hold 3 collision objects.
    std::vector<moveit_msgs::CollisionObject> collision_objects;
    collision_objects.resize(3);

    // Add the first table where the cube will originally be kept.
    collision_objects[0].id = "table1";
    collision_objects[0].header.frame_id = "world";

    /* Define the primitive and its dimensions. */
    collision_objects[0].primitives.resize(1);
    collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
    collision_objects[0].primitives[0].dimensions.resize(3);
    collision_objects[0].primitives[0].dimensions[0] = 0.2;
    collision_objects[0].primitives[0].dimensions[1] = 0.4;
    collision_objects[0].primitives[0].dimensions[2] = 0.4;

    /* Define the pose of the table. */
    collision_objects[0].primitive_poses.resize(1);
    collision_objects[0].primitive_poses[0].position.x = 0.5;
    collision_objects[0].primitive_poses[0].position.y = 0;
    collision_objects[0].primitive_poses[0].position.z = 0.2;
    collision_objects[0].primitive_poses[0].orientation.w = 1.0;
    // END_SUB_TUTORIAL

    collision_objects[0].operation = collision_objects[0].ADD;

    // BEGIN_SUB_TUTORIAL table2
    // Add the second table where we will be placing the cube.
    collision_objects[1].id = "table2";
    collision_objects[1].header.frame_id = "world";

    /* Define the primitive and its dimensions. */
    collision_objects[1].primitives.resize(1);
    collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
    collision_objects[1].primitives[0].dimensions.resize(3);
    collision_objects[1].primitives[0].dimensions[0] = 0.4;
    collision_objects[1].primitives[0].dimensions[1] = 0.2;
    collision_objects[1].primitives[0].dimensions[2] = 0.4;

    /* Define the pose of the table. */
    collision_objects[1].primitive_poses.resize(1);
    collision_objects[1].primitive_poses[0].position.x = 0;
    collision_objects[1].primitive_poses[0].position.y = 0.5;
    collision_objects[1].primitive_poses[0].position.z = 0.2;
    collision_objects[1].primitive_poses[0].orientation.w = 1.0;
    // END_SUB_TUTORIAL

    collision_objects[1].operation = collision_objects[1].ADD;

    // BEGIN_SUB_TUTORIAL object
    // Define the object that we will be manipulating
    collision_objects[2].header.frame_id = "world";
    collision_objects[2].id = "object";

    /* Define the primitive and its dimensions. */
    collision_objects[2].primitives.resize(1);
    collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
    collision_objects[2].primitives[0].dimensions.resize(3);
    collision_objects[2].primitives[0].dimensions[0] = 0.02;
    collision_objects[2].primitives[0].dimensions[1] = 0.02;
    collision_objects[2].primitives[0].dimensions[2] = 0.2;

    /* Define the pose of the object. */
    collision_objects[2].primitive_poses.resize(1);
    collision_objects[2].primitive_poses[0].position.x = 0.5;
    collision_objects[2].primitive_poses[0].position.y = 0;
    collision_objects[2].primitive_poses[0].position.z = 0.5;
    collision_objects[2].primitive_poses[0].orientation.w = 1.0;
    // END_SUB_TUTORIAL

    collision_objects[2].operation = collision_objects[2].ADD;

    planning_scene_interface.applyCollisionObjects(collision_objects);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pick_and_place");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    auto planning_scene_interface = std::make_shared<moveit::planning_interface::PlanningSceneInterface>();

    auto both_arms_mgi = std::make_shared<moveit::planning_interface::MoveGroupInterface>("both_arms");
    const moveit::core::JointModelGroup *both_arms_jmg = both_arms_mgi->getCurrentState()->getJointModelGroup("both_arms");

    auto right_arm_mgi = std::make_shared<moveit::planning_interface::MoveGroupInterface>("right_arm");
    const moveit::core::JointModelGroup *right_arm_jmg = right_arm_mgi->getCurrentState()->getJointModelGroup("right_arm");

    auto left_arm_mgi = std::make_shared<moveit::planning_interface::MoveGroupInterface>("left_arm");
    const moveit::core::JointModelGroup *left_arm_jmg = left_arm_mgi->getCurrentState()->getJointModelGroup("left_arm");

    auto right_gripper_mgi = std::make_shared<moveit::planning_interface::MoveGroupInterface>("right_gripper");
    const moveit::core::JointModelGroup *right_gripper_jmg = right_gripper_mgi->getCurrentState()->getJointModelGroup("right_gripper");

    auto left_gripper_mgi = std::make_shared<moveit::planning_interface::MoveGroupInterface>("left_gripper");
    const moveit::core::JointModelGroup *left_gripper_jmg = left_gripper_mgi->getCurrentState()->getJointModelGroup("left_gripper");

    right_arm_mgi->setMaxVelocityScalingFactor(1.0);
    right_arm_mgi->setMaxAccelerationScalingFactor(1.0);
    right_gripper_mgi->setMaxVelocityScalingFactor(1.0);
    right_gripper_mgi->setMaxAccelerationScalingFactor(1.0);

    left_arm_mgi->setMaxVelocityScalingFactor(1.0);
    left_arm_mgi->setMaxAccelerationScalingFactor(1.0);
    left_gripper_mgi->setMaxVelocityScalingFactor(1.0);
    left_gripper_mgi->setMaxAccelerationScalingFactor(1.0);

    namespace rvt = rviz_visual_tools;
    auto visual_tools = std::make_shared<moveit_visual_tools::MoveItVisualTools>("yumi_base_link");
    visual_tools->deleteAllMarkers();
    visual_tools->loadRemoteControl();

    // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
    text_pose.translation().z() = 0.75;
    visual_tools->publishText(text_pose, "PICK AND PLACE TESTING", rvt::WHITE, rvt::XXLARGE);
    visual_tools->trigger();

    addCollisionObjects(*planning_scene_interface);

    cout << "PICK.\n";
    pick(*left_arm_mgi, *left_gripper_mgi);

    cout << "PLACE.\n";
    place(*left_arm_mgi, *left_gripper_mgi);

    ros::shutdown();
    return 0;
}

Backtrace or Console output

[ INFO] [1690749233.026266461]: Added plan for pipeline 'pick'. Queue is now of size 1
The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling matrix diagonal differs from [1 1 1] by [ 1.3513e-06 1.18114e-06 1.28091e-06] but the required precision is 1e-12.
move_group: /home/rodpc/hri/src/moveit/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h:320: void collision_detection::transform2fcl(const Isometry3d&, fcl::Transform3d&): Assertion `!"Invalid isometry transform"' failed.
--Type <RET> for more, q to quit, c to continue without paging--

Thread 25 "move_group" received signal SIGABRT, Aborted.
[Switching to Thread 0x7fffb543f700 (LWP 617464)]
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50  ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff7293859 in __GI_abort () at abort.c:79
#2  0x00007ffff7293729 in __assert_fail_base (fmt=0x7ffff7429588 "%s%s%s:%u: %s%sAssertion `%s' failed.\n%n", 
    assertion=0x7ffff46e8f69 "!\"Invalid isometry transform\"", 
    file=0x7ffff46e8ef0 "/home/rodpc/hri/src/moveit/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h", line=320, function=<optimized out>) at assert.c:92
#3  0x00007ffff72a4fd6 in __GI___assert_fail (assertion=0x7ffff46e8f69 "!\"Invalid isometry transform\"", 
    file=0x7ffff46e8ef0 "/home/rodpc/hri/src/moveit/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h", line=320, function=0x7ffff46e8ea0 "void collision_detection::transform2fcl(const Isometry3d&, fcl::Transform3d&)")
    at assert.c:101
#4  0x00007ffff46b09a1 in collision_detection::transform2fcl (b=..., f=...)
    at /home/rodpc/hri/src/moveit/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h:320
#5  0x00007ffff46ac9ec in collision_detection::CollisionEnvFCL::constructFCLObjectRobot (this=0x55555632f6a0, state=..., fcl_obj=...)
    at /home/rodpc/hri/src/moveit/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp:208
#6  0x00007ffff46ad6b7 in collision_detection::CollisionEnvFCL::checkRobotCollisionHelper (this=0x55555632f6a0, req=..., res=..., 
    state=..., acm=0x7fffac00d550) at /home/rodpc/hri/src/moveit/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp:313
#7  0x00007ffff46ad1e9 in collision_detection::CollisionEnvFCL::checkRobotCollision (this=0x55555632f6a0, req=..., res=..., state=..., 
    acm=...) at /home/rodpc/hri/src/moveit/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp:290
#8  0x00007ffff5d1e887 in planning_scene::PlanningScene::checkCollision (this=0x55555566d620, req=..., res=..., robot_state=..., 
    acm=...) at /home/rodpc/hri/src/moveit/moveit_core/planning_scene/src/planning_scene.cpp:493
#9  0x00007fffc128d7df in planning_scene::PlanningScene::checkCollision (this=0x55555566d620, req=..., res=..., robot_state=..., 
    acm=...) at /home/rodpc/hri/src/moveit/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h:490
#10 0x00007fffc128bd67 in pick_place::ReachableAndValidPoseFilter::isEndEffectorFree (this=0x7fffac0118c0, plan=
    std::shared_ptr<struct pick_place::ManipulationPlan> (use count 1, weak count 0) = {...}, token_state=...)
    at /home/rodpc/hri/src/moveit/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp:103
#11 0x00007fffc128beee in pick_place::ReachableAndValidPoseFilter::evaluate (this=0x7fffac0118c0, 
    plan=std::shared_ptr<struct pick_place::ManipulationPlan> (use count 1, weak count 0) = {...})
    at /home/rodpc/hri/src/moveit/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp:111
#12 0x00007fffc127f6b9 in pick_place::ManipulationPipeline::processingThread (this=0x7fffac009f70, index=0)
    at /home/rodpc/hri/src/moveit/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp:174
#13 0x00007fffc127ee6f in pick_place::ManipulationPipeline::<lambda()>::operator()(void) const (__closure=0x7fffac016c28)
    at /home/rodpc/hri/src/moveit/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp:117
#14 0x00007fffc1281130 in boost::detail::thread_data<pick_place::ManipulationPipeline::start()::<lambda()> >::run(void) (
    this=0x7fffac016af0) at /usr/include/boost/thread/detail/thread.hpp:120
#15 0x00007ffff574143b in ?? () from /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
--Type <RET> for more, q to quit, c to continue without paging--c
#16 0x00007ffff7743609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#17 0x00007ffff7390133 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
welcome[bot] commented 1 year ago

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

rodpc07 commented 1 year ago

After some tinkering, I found that this crash is the result of building the workspace with -DCMAKE_BUILD_TYPE set to DEBUG, since it doesn't crash when set to RELEASE. I don't know if the release build is hiding an underlying problem that was only caught in debug.

rhaschke commented 1 year ago

Your console output is very explicit about the underlying error: The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling matrix diagonal differs from [1 1 1] by [ 1.3513e-06 1.18114e-06 1.28091e-06] but the required precision is 1e-12. Do you feed any transforms with float precision only (instead of double)?

The reason that the crash only occurs in DEBUG mode, is that we add some extra checks in debug mode. So, that's expected.

rhaschke commented 1 year ago

By the way, the recommended way to plan pick+place actions is to use MTC: https://ros-planning.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html

rodpc07 commented 1 year ago

Do you feed any transforms with float precision only (instead of double)?

Excuse my ignorance; to my knowledge, I am only running demo.launch from my yumi_moveit_config and the node from the code above, in which I don't believe I used any floats. That's why I am confused about what is causing this. If I run this in release, will it cause any problems?

By the way, the recommended way to plan pick+place actions is to use MTC: https://ros-planning.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html

I appreciate the advice and will be sure to use it in future applications, but for my project, I don't need anything much more complex than what is shown at the pick-and-place tutorial and don't have the time to study and tinker with how to implement the MTC for a dual-arm robot.

rhaschke commented 1 year ago

If I run this in release, will it cause any problems?

These tiny deviations will not cause any issues.