Path constraints

asked 2017-11-14 04:54:16 -0600

pdmike gravatar image

updated 2017-11-14 04:55:06 -0600

Hello I am trying to keep the end effector of the UR10 arm parallel to the ground using Moveit, which means the pose of end effector's pitch=0, roll=0 and yaw angle can be any value. My code is shown as followed and the motion plan can't be found, Fail: ABORTED: No motion plan found. No execution attempted, . And i am sure the pose 'next_pose" is reachable. Would you mind checking the code and pointing out the mistake?

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


    /* This sleep is ONLY to allow Rviz to come up */
    sleep(20.0);

    moveit::planning_interface::MoveGroup group_r("arm");
    group_r.setPlannerId("RRTConnectkConfigDefault");
    group_r.setPlanningTime(45);
//ocm
    moveit_msgs::OrientationConstraint ocm;

    ocm.link_name = "tt_base_link";
    ocm.header.frame_id = "base_link";

    ocm.orientation.x = 0.210738;
    ocm.orientation.y = 0.672778;
    ocm.orientation.z = 0.677635;
    ocm.orientation.w = 0.209212;

    ocm.absolute_x_axis_tolerance = 0.01;
    ocm.absolute_y_axis_tolerance = 0.01;
    ocm.absolute_z_axis_tolerance = 3.14;
    ocm.weight = 1.0;
    moveit_msgs::Constraints test_constraints;
    test_constraints.orientation_constraints.push_back(ocm);
    group_r.setPathConstraints(test_constraints);

//start pose


   geometry_msgs::Pose start_pose;
    start_pose.position.x = -0.402141;
    start_pose.position.y = 0.162843;
    start_pose.position.z = 0.384870;
    start_pose.orientation.x = -0.003270;
    start_pose.orientation.y = 0.704558;
    start_pose.orientation.z = 0.709631;
    start_pose.orientation.w = -0.003249;


    const robot_state::JointModelGroup *joint_model_group =
                    //start_state.getJointModelGroup(group_r.getName());
    start_state.setFromIK(joint_model_group, start_pose);
    group_r.setStartState(start_state);

// target  
    geometry_msgs::Pose next_pose;



        next_pose.position.x =-0.402141;
        next_pose.position.y =0.462843;
        next_pose.position.z =0.384870;
        next_pose.orientation.x =0.167744;
        next_pose.orientation.y =0.684582;
        next_pose.orientation.z =0.689516;
        next_pose.orientation.w =0.166666;

        group_r.setPoseTarget(next_pose,"tt_base_link");


    moveit::planning_interface::MoveGroup::Plan my_plan;
    bool success = group_r.plan(my_plan);

    ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");


        sleep(5.0);
    group_r.clearPathConstraints();
}
edit retag flag offensive close merge delete

Comments

Hey @pdmike! I know the question is quite old, but have you found the source of the problem? I am also trying to move a UR10 using similar constraints and more often than not the plan fails.

geotsam gravatar image geotsam  ( 2021-02-10 02:42:29 -0600 )edit

Could you post all the output from that program? Does it complain about the start state? Are you sure the robot's start state is exactly the same as the one you explicitly specify? Why not get current state and then set the start state from that?

McMurdo gravatar image McMurdo  ( 2021-07-26 05:06:38 -0600 )edit