move_group.setWorkspace() doesn't work?

edit

Hi all!

I am working on a project where I have to build lego structures collaberative with a human. Now I am new to moveit and have the cartesian path planning working in gazebo (sinusoidals, pick and place movement).

I was trying to use the move_group.setWorspace() function and for some reason it just doesn't give this information to the planning_scene_interface. When I execute a trajectory outside of it's workspace it will just go there for no reason. Also i wanted to implement anti-collision using octomapping however I don't think it will be a good idea to start on that if i can't control the size of my workspace properly.

PS: I also have changed the workspace settings in the moveit.rviz file from the ur5, I can see the workspace getting these dimensions in rviz however the ur5 will still go there.

Kind regards, Edwin van Emmerik CODE:

int main(int argc, char **argv) { ros::init(argc, argv, "trajectory_tester"); ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); spinner.start(); // Construct planning scene for the move_group const std::string PLANNING_GROUP = "manipulator"; moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); move_group.setWorkspace(-0.2, -0.2, 0.05, 1, 1, 1); // set workspace size for move_group robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); // Make sure that the robot model gets loaded if (robot_model != 0) { ROS_INFO("Robot model loaded"); } else { ROS_INFO("Could not load robot model"); }

// Get the planning frames
ROS_INFO("Reference frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO("Reference frame: %s", move_group.getEndEffectorLink().c_str());
move_group.setPlannerId("PRMstarkConfigDefault"); //default planner is set to RTTCONNECT in ompl_planning_.yaml
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
/*
int chosenone;
std::cout << "You can choose between pick and place(1) and sinusoidal movements(2)" << std::endl;
std::cin >> chosenone;
std::cout << std::endl;
*/
// Construct target_pose object of type geometry_msgs
// Construct waypoints vector of type geometry_msgs::Pose
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose start_pose = move_group.getCurrentPose().pose;
geometry_msgs::Pose target_pose = start_pose;

        // Keep initiazlize state and also keep the UR5 facing downwards for default
        target_pose.position.x = 0.6;
        target_pose.position.y = 0.05;
        target_pose.position.z = 0.1;
        target_pose.orientation.w = 0.707;
        target_pose.orientation.x = 0;
        target_pose.orientation.y = 0.707;
        target_pose.orientation.z = 0;
        move_group.setPoseTarget(target_pose);
        move_group.plan(my_plan);
        move_group.move();


        int number_of_waypoints = 7;
        double x[number_of_waypoints];
        x[0] = 0.5;
        x[1] = 0.0;
        x[2] = 0.0;
        x[3] = 0.0;
        x[4] = 0.5;
        x[5] = 0.5;
        x[6] = 0.5;
        double y[number_of_waypoints];
        y[0] = 0.0;
        y[1] = 0.5;
        y[2] = 0.5;
        y[3] = 0.5;
        y[4] = 0.0;
        y[5] = 0.0;
        y[6] = 0.0;
        double z[number_of_waypoints];
        z[0] = 0.4;
        z[1] = 0.4;
        z[2] = 0.1;
        z[3] = 0.4;
        z[4] = 0.4;
        z[5] = 0.1;
        z[6] = 0.4;
        for (int i = 0; i < number_of_waypoints; i++)
        {
            target_pose.position.x = x[i];
            target_pose.position.y = y[i];
            target_pose.position.z = z[i];
            waypoints.push_back(target_pose);
        }

/* NOTE THAT ... (more)