ros-industrial / stomp_ros

ROS packages for the STOMP planner (split out of industrial_moveit)
Apache License 2.0
37 stars 27 forks source link

STOMP can't plan to a pose goal #32

Closed migueelnery closed 4 years ago

migueelnery commented 4 years ago

Hi! I am trying to plan using a pose goal with STOMP in a simulated UR5 with Moveit. The planner has sucess when I use Joint Position Goals, but when I try to use Pose Goals I got the following error:

[ERROR] [1597078131.640680192, 51.345000000]: A valid ik solution for the given Cartesian constraints was not found 
[ERROR] [1597078131.640721378, 51.345000000]: STOMP was unable to retrieve the goal from the MotionPlanRequest

This is the code that I am using : (it works with Pose and Joint Goals when the planner is OMPL, but when I use STOMP it only works with Joint Goals).

    robot_state::RobotState start_state(*move_group.getCurrentState());
    move_group.setStartState(start_state);
    //POSE
    geometry_msgs::Pose target_pose;
    target_pose.orientation.w = 0.474989; 
    target_pose.orientation.x = -0.522257;
    target_pose.orientation.y = 0.47662;  
    target_pose.orientation.z = -0.523895; 
    target_pose.position.x = 0.000603714;
    target_pose.position.y = -0.335967;
    target_pose.position.z = 0.305978;
    move_group.setPoseTarget(target_pose);
    //JOINTS
    //  moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
    //  std::vector<double> joint_group_positions;
    //  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
    //  joint_group_positions[0] = -2.13083290537;  
    //  joint_group_positions[1] = -1.4019123091;  
    //  joint_group_positions[2] = 2.2600063779; // 
    //  joint_group_positions[3] = 2.28247727295;   
    //  joint_group_positions[4] = 0.557602172179;
    //  joint_group_positions[5] = 6.18913213329;
    //  move_group.setJointValueTarget(joint_group_positions);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    move_group.plan(my_plan);
    move_group.execute(my_plan);

Thanks for any help!