Ask Your Question
0

Planning error with path constrains

asked 2015-10-13 09:52:15 -0500

updated 2015-10-20 04:33:54 -0500

I'm using Moveit (0.6.15-Alpha) in ROS Indigo to control an arm. The planning goes well untill I try to plan a new position which has some path constrains (orientation and position on two axis of the end effector).

The code looks like this:

    // Orientation constrain adding
    moveit_msgs::Constraints flip_constrains;
    moveit_msgs::OrientationConstraint flip_orientation;
    flip_orientation.link_name = eef;
    flip_orientation.header.frame_id = base_reference;
    flip_orientation.orientation = move_group_->getCurrentPose().pose.orientation;              
    flip_orientation.absolute_x_axis_tolerance = 0.15;
    flip_orientation.absolute_y_axis_tolerance = 0.15;
    flip_orientation.absolute_z_axis_tolerance = 0.15;
    flip_orientation.weight = 0.7;
    flip_constrains.orientation_constraints.push_back(flip_orientation);
    // Position constrain adding
    moveit_msgs::PositionConstraint flip_position;
    flip_position.link_name = eef;
    flip_position.header.frame_id = base_reference;
    flip_position.target_point_offset.x = 0; //target.position.x;
    flip_position.target_point_offset.y = target.position.y;
    flip_position.target_point_offset.z = target.position.z;
    flip_position.weight = 1.0;
    flip_constrains.position_constraints.push_back(flip_position);
    move_group_->setPathConstraints(flip_constrains);
    target.position.x = req.distance;
    target.orientation = move_group_->getCurrentPose().pose.orientation;

    move_group_->setPoseTarget(target);
move_group_->setStartState(*move_group_->getCurrentState());

bool success = move_group_->plan(move_plan);
ROS_INFO("Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
sleep(5.0);
if (success) {
    move_group_->move(); //execute(move_plan);
} else {
    ROS_ERROR(
            "Error while planning position of arm, no move command was sent!\n");
    return false;
}
sleep(2.0);

There are to outcomes of this code. The most crucial is when moveit exits with error:

[ INFO] [1444747356.697432845]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ WARN] [1444747356.697539343]: Execution of motions should always start at the robot''s current state. Ignoring the state supplied as start state in the motion planning request
[ INFO] [1444747356.697561413]: Planning attempt 1 of at most 1
[ INFO] [1444747356.697634046]: Starting state is just outside bounds (joint 'elbow_pitch_joint'). Assuming within bounds.
[ INFO] [1444747356.704333992]: Loading robot model 'cyton_gamma_1500'...
[ INFO] [1444747356.704373020]: No root joint specified. Assuming fixed joint
[ INFO] [1444747356.774530395]: No planner specified. Using default.
[ INFO] [1444747356.774593336]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1444747356.774859013]: Allocating specialized state sampler for state space
[ INFO] [1444747356.774898663]: LBKPIECE1: Starting planning with 1 states already in datastructure
***** Internal Program Error - assertion (satisfiesBounds(state1) && satisfiesBounds(state2)) failed in virtual double ompl::base::SO3StateSpace::distance(const ompl::base::State*, const ompl::base::State*) const:
/tmp/buildd/ros-indigo-ompl-1.0.0003094-0trusty-20141029-0351/src/ompl/base/spaces/src/SO3StateSpace.cpp(284): The states passed to SO3StateSpace::distance are not within bounds. Call SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, ompl::control::StatePropagator, or ompl::base::StateValidityChecker
[move_group-7] process has died [pid 1099, exit code -6, cmd /opt/ros/indigo/lib/moveit_ros_move_group/move_group __name:=move_group __log:=/home/elod/.ros/log/61d301e0-71b8-11e5-b90c-2c44fd0c995f/move_group-7.log].
log file: /home/elod/.ros/log/61d301e0-71b8-11e5-b90c-2c44fd0c995f/move_group-7*.log

Or moveit throws an unmet constrain error where the eef string is the camera_front:

[ INFO] [1444746408.872008469]: Planning adapters have added states at index positions: [ 0 ]
[ERROR] [1444746408.873127307]: Computed path is not valid. Invalid states at index locations: [ 3 4 5 6 7 8 9 ] out of 11. Explanations follow in command line. Contacts are published on /move_group/display_contacts
[ INFO] [1444746408.873301981]: Orientation constraint violated for link ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-10-21 05:08:43 -0500

I fond a workaround. On the official sites is written that the LBKPIECE1 is experimental and if no planner ID is set, than this is the default. So, I changed the planner to RRT which works well without crashing moveit.

move_group_->setPlannerId("RRTConnectkConfigDefault");
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2015-10-13 09:52:15 -0500

Seen: 999 times

Last updated: Oct 21 '15