No solution found moveit C++

[sasilva1998]

Hello everyone,

I am trying to move the pepper robot using a C++, here is my code:

#include <iostream>
#include <vector>

#include <cmath>
#include <cstdlib>
#include <string>

// ROS stuff
#include <ros/ros.h>

#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>

#include <geometry_msgs/PoseStamped.h>
#include <moveit/robot_state/robot_state.h>

#include <control_msgs/GripperCommandAction.h>
#include <control_msgs/PointHeadAction.h>
#include <control_msgs/PointHeadGoal.h>
#include <geometry_msgs/TransformStamped.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <actionlib/client/simple_action_client.h>

//!  PepperMoveItDemo class.
/*!
 * Pepper MoveIt! Demo.
 *
 */
class Pepper
{
public:
    //! Constructor
    Pepper();
    //! Function for waiting for user confirmation
    void waitForUserConfirmation(const std::string &next_task);
    //! Run
    void run();

private:
    // ROS
    ros::NodeHandle node_handler_, local_nh_;
    bool replan_;
};

Pepper::Pepper() : local_nh_("~"), replan_(false)
{
    //=======================================================================
    // Subscribers
    //=======================================================================
}

void Pepper::run()
{
    ros::AsyncSpinner spinner(4);
    spinner.start();

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    moveit::planning_interface::MoveGroupInterface move_group_right("right_arm");
    moveit::planning_interface::MoveGroupInterface move_group_left("left_arm");

    move_group_right.setPlannerId("RRTConnectkConfigDefault");
    move_group_left.setPlannerId("RRTConnectkConfigDefault");
    auto pcm = planning_scene_monitor::PlanningSceneMonitorPtr(
        new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
    planning_scene_monitor::LockedPlanningSceneRO planning_scene(pcm);

    // Open the gripper before starting
    moveit::planning_interface::MoveItErrorCode success =
        moveit::planning_interface::MoveItErrorCode::FAILURE;

    ros::Rate loop_rate(10);

    //=======================================================================
    // Replanning if needed
    //=======================================================================
    replan_ = true;
    std::string arm = "right";

    while (ros::ok())
    {
        if (replan_)
        {
            replan_ = true;

            // Pregrasp
            geometry_msgs::PoseStamped target_pose;

            target_pose.pose.position.y = 0.0;
            target_pose.pose.position.z = 0.6;
            target_pose.header.frame_id = "base_footprint";
            if (arm.compare("right") == 0)
            {
                target_pose.pose.position.x = 0.05;
                target_pose.pose.position.y = -0.19;
                target_pose.pose.position.z = 0.65;
                target_pose.pose.orientation.x = 0.074;
                target_pose.pose.orientation.y = 0.19;
                target_pose.pose.orientation.z = -0.10;
                target_pose.pose.orientation.w = 0.97;
                move_group_right.setPoseTarget(target_pose);
                move_group_right.setGoalTolerance(0.05);
            }
            else
            {
                target_pose.pose.position.x = 0.28;
                target_pose.pose.position.y = 0.16;
                target_pose.pose.position.z = 0.91;
                target_pose.pose.orientation.w = 1;
                move_group_left.setPoseTarget(target_pose);
            }

            if (arm.compare("right") == 0)
            {
                success = move_group_right.plan(my_plan);
            }
            else
            {
                success = move_group_left.plan(my_plan);
            }

            if (success == moveit::planning_interface::MoveItErrorCode::SUCCESS)
            {
                if (arm.compare("right") == 0)
                {
                    move_group_right.execute(my_plan);
                }
                else
                {
                    move_group_left.execute(my_plan);
                }
            }
            loop_rate.sleep();
            sleep(1.0);

            if (!success)
                continue;
            if (arm.compare("right") == 0)
            {
                arm = "left";
            }
            else
            {
                break;
            }
        }
        // std::cout << "Waiting the user to move close."<< std::endl;
        loop_rate.sleep();
    }

    spinner.stop();
    ros::shutdown();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pepper_moveit_demo");

    Pepper demo;
    demo.run();

    return 0;
}

however I am getting the following errors:

ERROR][/move_group][1637102624.110187292, 56.718000000]: right_arm[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO][/move_group][1637102624.110497290, 56.718000000]: right_arm[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ INFO][/move_group][1637102624.110581027, 56.718000000]: No solution found after 5.001536 seconds
[ INFO][/move_group][1637102624.127406109, 56.724000000]: Unable to solve the planning problem
[ WARN][/move_group][1637102624.127755271, 56.724000000]: The complete state of the robot is not yet known.  Missing LHand, RHand, WheelB, WheelFL, WheelFR
[ INFO][/move_group][1637102625.165723672, 57.182000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO][/move_group][1637102625.165921776, 57.182000000]: Starting state is just outside bounds (joint 'RElbowRoll'). Assuming within bounds.
[ WARN][/move_group][1637102625.166422461, 57.182000000]: Orientation constraint for link 'r_wrist' is probably incorrect: 0.074000, 0.190000, -0.100000, 0.970000. Assuming identity instead.
[ WARN][/move_group][1637102625.166637108, 57.182000000]: Orientation constraint for link 'r_wrist' is probably incorrect: 0.074000, 0.190000, -0.100000, 0.970000. Assuming identity instead.
[ INFO][/move_group][1637102625.166951341, 57.182000000]: Planner configuration 'right_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO][/move_group][1637102625.167426638, 57.182000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
Joint 'virtual_odom' not found in model 'JulietteY20MP'

If anybody new why it is happening or provide an example it would be very helpful. I have tried many different things.

thank you.

[sasilva1998]

Could someone assure if inverse kinemtics works ok on the robot simulation?

[sasilva1998]

I have solved the problem by using approximate solution as follows:

move_group_left.setPoseReferenceFrame("base_link");
move_group_left.setGoalTolerance(0.1);
move_group_left.setGoalOrientationTolerance(0.1);
move_group_left.setPoseTarget(target_pose);
move_group_left.setApproximateJointValueTarget(target_pose, "l_wrist");