setFromIK finds solution, but no path can be found

[Karel Vanhoorebeeck]

Hello everybody,

I'm trying to reach a position with the youBot arm. For this I use the code below:

void in_some_method(){    
    calc_pitch(best_xyz, object, pitch, dist); //calculates the required pitch
    //check if IK solution exists
    tf::Quaternion quat;
    quat.setRPY(roll, pitch, yaw);

    printf("Best x %f, best y %f, best z %f.\n", best_xyz[XI], best_xyz[YI], best_xyz[ZI]); // values were found with exhaustive search for now
    printf("Fitness: %f, Height: %f, Distance: %f\n", fitness, best_xyz[ZI], dist);

    geometry_msgs::PoseStamped proposed_pose;
    load_pose(proposed_pose, best_xyz, quat); //simply copies the values into a Pose Msg

    bool found_ik = robot_state_ptr->setFromIK(joint_model_group, proposed_pose.pose, 10, 1);
    ROS_INFO_STREAM("IK Solution found for proposed pose: " << found_ik);
    ROS_INFO_STREAM("Roll: " << roll/M_PI << "PI Pitch " << pitch/M_PI << "PI Yaw: " << yaw/M_PI << "PI");

    group.setPoseTarget(proposed_pose, group.getEndEffectorLink());
    group.setGoalOrientationTolerance(0.1);//pretty flexible, no?
    group.setGoalPositionTolerance(0.1);
    moveit::planning_interface::MoveGroup::Plan plan;
    group.plan(plan);
    group.execute(plan);
}


void load_pose(geometry_msgs::PoseStamped& pose, double p_pos[], tf::Quaternion q){
    pose.pose.orientation.x = q.getX();
    pose.pose.orientation.y = q.getY();
    pose.pose.orientation.z = q.getZ();
    pose.pose.orientation.w = q.getW();

    pose.pose.position.x = p_pos[XI];
    pose.pose.position.y = p_pos[YI];
    pose.pose.position.z = p_pos[ZI];
    pose.header.frame_id = "base_footprint";
//    ROS_INFO_STREAM("Pose Frame ID: " << pose.header.frame_id);

}

Gives the following output:

[ INFO] [1436188845.586666958]: Loading robot model 'youbot'...
[ INFO] [1436188846.887047233]: Ready to take MoveGroup commands for group arm_1.
[ INFO] [1436188846.900239730]: Loading robot model 'youbot'...
Yaw is: 0.250000PI
Best x 0.212132, best y 0.212132, best z 0.400000.
Fitness: 1.436141, Height: 0.400000, Distance: 1.414214
[ INFO] [1436188847.597321481]: IK Solution found for proposed pose: 1
[ INFO] [1436188847.597365393]: Roll: 0PI Pitch 0.58774PI Yaw: 0.25PI
[ WARN] [1436188847.707050096]: Fail: ABORTED: No motion plan found. No execution attempted.

And the following output in The MoveIt terminal window:

[ INFO] [1436188847.598602813]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1436188847.603681705]: No planner specified. Using default.
[ INFO] [1436188847.603814546]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1436188847.605693083]: LBKPIECE1: Starting with 1 states
[ERROR] [1436188847.706522437]: LBKPIECE1: Unable to sample any valid states for goal tree
[ INFO] [1436188847.706558765]: LBKPIECE1: Created 1 (1 start + 0 goal) states in 1 cells (1 start (1 on boundary) + 0 goal (0 on boundary))
[ INFO] [1436188847.706582057]: No solution found after 0.102657 seconds
[ INFO] [1436188847.706679937]: Unable to solve the planning problem
[ INFO] [1436188847.707963076]: Received new trajectory execution service request...
[ WARN] [1436188847.708034829]: The trajectory to execute is empty

Does anyone know what the problem here might be? I check for an IK solution, but for some reason it does not find a path... I do have my doubts about the actual reachability of the found pose (40cm in z-direction is quite high for the youbot, especially with the x and y values considered too. But then, what am I doing wrong when checking for an IK solution?

Thank you for your time!

I'm using ROS Hydro on Ubuntu 12.04.

[Karel Vanhoorebeeck]

Since the problem might be related to the way I call the IK solver, here is my full code. It may give you all some more insight.

#define XI 0
#define YI 1
#define ZI 2

#define PI 0
#define QI 1

double roll = 0, pitch = 0, yaw = 0;
double object[] = {0.2, 1, 0};

int main(int argc, char **argv)
{
    ros::init (argc, argv, "motion_planning");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle node_handle("move_group");
//    ros::Publisher robot_state_publisher = node_handle.advertise<moveit_msgs::DisplayRobotState>( "monitored_planning_scene", 1 );

    moveit::planning_interface::MoveGroup group("arm_1");

    robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
    robot_model::RobotModelPtr robot_model_ptr = robot_model_loader.getModel();
    robot_state::RobotStatePtr robot_state_ptr(group.getCurrentState()); //copy state
    robot_state_ptr->setToDefaultValues();
//    group.setStartState(*robot_state_ptr);

    const robot_state::JointModelGroup* joint_model_group = robot_model_ptr->getJointModelGroup(group.getName());
    const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();


    yaw = atan2(object[YI], object[XI]);
    printf("Yaw is: %fPI\n", yaw/M_PI);

    //now exhaustively search in plane determined by yaw-direction (p-axis) and z axis (q-axis)
    double lb[] = {0, 0}; //lower boundary
    double ub[] = {0.5, 0.5}; // upper boundary
    double stepsize = 0.05; //in m
//    int psteps = (ub[PI] = lb[PI])/stepsize; //watch out for non-integer values
//    int qsteps = (ub[QI] = lb[QI])/stepsize;

    double best_fitness = -1;
    double best_xyz[3] = {0};
    double fitness = -1;
    double dist = 0;

    for(double p = lb[PI]; p < ub[PI]; p+= stepsize){
        for(double q = lb[QI]; q < ub[QI]; q+= stepsize){
            //Map point to xyz coordinates and calculate pitch, can be done directly later

//            printf("p: %f, q: %f\n", p, q);
            double pos[3] = {p*cos(yaw), p*sin(yaw), q};
//            ROS_INFO_STREAM("pos x " << pos[XI] << " pos y " << pos[YI] << " pos z " << pos[ZI]);
            calc_pitch(pos, object, pitch, dist);

            tf::Quaternion quat;
            quat.setRPY(roll, pitch, yaw);

            geometry_msgs::PoseStamped proposed_pose;
            load_pose(proposed_pose, pos, quat);
//            ROS_INFO_STREAM("pos x " << pos[XI] << " pos y " << pos[YI] << " pos z " << pos[ZI]);
//            ROS_INFO_STREAM("Roll: " << roll/M_PI << "PI Pitch " << pitch/M_PI << "PI Yaw: " << yaw/M_PI << "PI");

            bool found_ik = robot_state_ptr->setFromIK(joint_model_group, proposed_pose.pose, 10, 1);

            if(found_ik == 1){
                fitness = sqrt(pow(dist,2) + pow(pos[ZI]*10,2));
//                ROS_INFO_STREAM("IK Solution found for target_pose1. Fitness:  " << fitness);
                if(fitness >= best_fitness){
//                    printf("Updating best fitness\n");

                    best_fitness = fitness;
                    best_xyz[XI] = pos[XI];
                    best_xyz[YI] = pos[YI];
                    best_xyz[ZI] = pos[ZI];
//                    ROS_INFO_STREAM("best x " << best_xyz[XI] << " best y " << best_xyz[YI] << " best z " << best_xyz[ZI]);

//                    ROS_INFO_STREAM("Best fitness:  " << fitness << " Height: " << q << "Dist: " << dist);

                }

            }

        }
    }
    calc_pitch(best_xyz, object, pitch, dist);

    //check if IK solution exists
    tf::Quaternion quat;
    quat.setRPY(roll, pitch, yaw);

    geometry_msgs::PoseStamped proposed_pose;

    printf("Best x %f, best y %f, best z %f.\n", best_xyz[XI], best_xyz[YI], best_xyz[ZI]);

    printf("Fitness: %f, Height: %f, Distance: %f\n", fitness, best_xyz[ZI], dist);

    load_pose(proposed_pose, best_xyz, quat);

    bool found_ik = robot_state_ptr->setFromIK(joint_model_group, proposed_pose.pose, 10, 1);
    ROS_INFO_STREAM("IK Solution found for proposed pose: " << found_ik);

//    ROS_INFO_STREAM("Proposed pose: " << proposed_pose);

    ROS_INFO_STREAM("Roll: " << roll/M_PI << "PI Pitch " << pitch/M_PI << "PI Yaw: " << yaw/M_PI << "PI");

    moveit::planning_interface::MoveGroup::Plan plan;

    group.setGoalOrientationTolerance(0.1);
    group.setGoalPositionTolerance(0.1);
    group.setPoseTarget(proposed_pose);
    group.plan(plan);
    group.execute(plan);
}


void calc_pitch(double pos[], double object[], double& pitch, double& dist){
    //calcuate distance between origin and object
    dist = sqrt(pow(object[XI], 2) + pow(object[YI], 2)); //assumed to be z = zero
    double height = pos[ZI];
    double pitch_first = atan(dist/height); // should be in range [0, pi/2], so substract pi/2 (pitch of 0 = looking UP for the youbot. calcualtion: 0 = looking forward
    pitch = M_PI - pitch_first;
//    printf("Pitch_first is: %fPI, pitch is %fPI\n", pitch_first/M_PI, pitch/M_PI);

[Simon Schmeißer]

Hi Karel,

I haven't checked your whole code, but here is a misconception I had a few days ago:  setFromIK does not actually check for collisions. You have to pass a function pointer (boost bind) if you want this

bool isStateValid(const planning_scene::PlanningScene *planning_scene,
                  robot_state::RobotState *state,
                  const robot_state::JointModelGroup *group, const double *ik_solution)
{
  state->setJointGroupPositions(group, ik_solution);
  state->update();
  return (!planning_scene || !planning_scene->isStateColliding(*state, group->getName()));
}

robot_state::GroupStateValidityCallbackFn constraint_fn;
constraint_fn = boost::bind(&isStateValid, scene, _1, _2, _3);
bool possible = state.setFromIK(state.getJointModelGroup("manipulator"), pose, 0, 0, constraint_fn);

Maybe that helps already

Another thing I would try is calling

std::vector<double> jointValues;
state->copyJointGroupPositions("manipulator", jointValues);
group.setJointValueTarget(jointValues);

to ensure that you get the same IK-solution

[Karel Vanhoorebeeck]

Thank you very much for your answer.

I'll try your first suggestion ASAP. Inspired by your second suggestion I tried to use

bool found_ik = robot_state_ptr->setFromIK(joint_model_group, proposed_pose.pose, 10, 1);

robot_state_ptr->copyJointGroupPositions("arm_1", joint_values);
group.setJointValueTarget(joint_values);
moveit::planning_interface::MoveGroup::Plan plan2;
group.setGoalJointTolerance(0.01);
group.plan(plan2);
group.execute(plan2);

This does find a path, but the following (setting the pose target) stil ldoes not.

bool found_ik = robot_state_ptr->setFromIK(joint_model_group, proposed_pose.pose, 10, 1);

robot_state_ptr->update(true);
bool bounds_ok  = robot_state_ptr->satisfiesBounds(0.0);

   
moveit::planning_interface::MoveGroup::Plan plan;

group.setGoalOrientationTolerance(0.01);
group.setGoalPositionTolerance(0.01);
group.setPoseTarget(proposed_pose);

moveit::planning_interface::MoveItErrorCode errorcode = group.plan(plan);
group.execute(plan);

Do you have any idea of possible differences between planning for joint values vs planning to a pose?