New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Got the error "Goal state violates joint limits", when planing path with CHOMP for Yumi #822

Open
Gene0213 opened this Issue Apr 5, 2018 · 12 comments

Comments

Projects
None yet
4 participants
@Gene0213
Copy link

Gene0213 commented Apr 5, 2018

Description

I got the error "Goal state violates joint limits", whenever I plan a path using CHOMP for the Yumi robot. I have checked out that all the joints are within the limits.

Your environment

  • ROS Distro: [Kinetic]
  • OS Version: e.g. Ubuntu 16.04
  • Source or Binary build? Source
  • If source, which git commit or tag?
    kinetic-devel, 0.9.11

Steps to reproduce

Setup CHOMP for the Yumi robot, then plan a path.

Expected behavior

A path should be created and can be seen in Rviz

Actual behaviour

Got the error: Goal state violates joint limits
Failed to create a path

Backtrace or Console output

[ INFO] [1522891377.564087186]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
Start State: 0.746477
[ INFO] [1522891377.564418719]: Setting joint yumi_joint_1_l to position -1.57518
[ INFO] [1522891377.564462225]: Setting joint yumi_joint_2_l to position -1.29051
[ INFO] [1522891377.564488868]: Setting joint yumi_joint_7_l to position 0.398028
[ INFO] [1522891377.564517429]: Setting joint yumi_joint_3_l to position 0.419941
[ INFO] [1522891377.564532115]: Setting joint yumi_joint_4_l to position 1.86984
[ INFO] [1522891377.564546408]: Setting joint yumi_joint_5_l to position 2.24122
[ INFO] [1522891377.564559617]: Setting joint yumi_joint_6_l to position -0.494349
[ERROR] [1522891377.564600561]: Goal state violates joint limits
[ WARN] [1522891377.579027190]: Fail: ABORTED: Unknown event

I have checked out that all the position are within the limits.

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented Apr 5, 2018

@Gene0213

This comment has been minimized.

Copy link

Gene0213 commented Apr 5, 2018

Hi bmagyar, sure. I tried to plan a path for the left arm (7 joints) only, and located the error message in "/moveit/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp"

  if (not planning_scene->getRobotModel()->satisfiesPositionBounds(goal_state.data()))
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

I print out the goal_state

Goal_state_names: [yumi_joint_1_l, yumi_joint_2_l, yumi_joint_7_l, yumi_joint_3_l, yumi_joint_4_l, yumi_joint_5_l, yumi_joint_6_l]
Goal_state_positions: [ -1.57518  -1.29051  0.398028  0.419941    1.86984   2.24122 -0.494349]

I have confirmed that it is within the limits. I also locate the function "satisfiesPositionBounds()" at "/moveit/moveit_core/robot_model/src/robot_model.cpp"

bool moveit::core::RobotModel::satisfiesPositionBounds(const double* state,
                                                       const JointBoundsVector& active_joint_bounds,
                                                       double margin) const
{
  assert(active_joint_bounds.size() == active_joint_model_vector_.size());
  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
    if (!active_joint_model_vector_[i]->satisfiesPositionBounds(state + active_joint_model_start_index_[i],
                                                                *active_joint_bounds[i], margin))
    {
      cout << "Joint " << i << " is not satisfied" << endl;
      return false;
    }
  return true;
}

The line starting with "cout" is added by me to check out which one isn't satisfied. I found out that sometimes I got the number 8 joint not satisfied, even though my "goal_state" has only 7 joints.

The joint_limits for the left arm is here. the full limitation for the robot is attached. joint_limits.yaml.zip
joint_limits:
yumi_joint_1_l:
min_position: -2.84
max_position: 2.84
yumi_joint_2_l:
min_position: -2.4
max_position: 0.65
yumi_joint_3_l:
min_position: -2
max_position: 1.29
yumi_joint_4_l:
min_position: -4.9
max_position: 4.9
yumi_joint_5_l:
min_position: -1.43
max_position: 2.3
yumi_joint_6_l:
min_position: -3.89
max_position: 3.89
yumi_joint_7_l:
min_position: -2.84
max_position: 2.84

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 16, 2018

I am currently trying to address this issue using the the yumi_moveit_config code after cloning yumi into my catkin workspace. I am working on a fork of this to address this issue. After an initial inspection,

  • I found there are a total of 16 joints stored in the active_joint_model_vector_.size() variable, however only 7 joints in the goal states.
  • The values in the join_limits.yaml file were changed to -99999 to +99999 to logically have the goal state within limits, but the error of Goal state violates joint limit still persists implying some messing up of the joint names in one of the yumi_config files. I am using the joint_limits.yaml file same as used by @Gene0213 but changed the limits to do some testing.
  • I tried to modify the satisfiesPositionBounds() function as shown below, not much luck here, sometimes link 8 does not satisy, sometimes link 13,15,4, etc. :
bool RobotModel::satisfiesPositionBounds(const double* state, const JointBoundsVector& active_joint_bounds,
                                         double margin) const
{
    assert(active_joint_bounds.size() == active_joint_model_vector_.size());
    
    for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i) 
    {
        std::cout << "TOTAL Joints: " << active_joint_model_vector_.size() << std::endl;
        std::cout << i  << ". "
                  << std::endl <<"Joint Name " << active_joint_model_vector_[i]->getName() << std::endl
                  << "Joint Type Name" << active_joint_model_vector_[i]->getTypeName() << std::endl
                  << "Joint Type " << active_joint_model_vector_[i]->getType() << std::endl << std::endl;
       
        
        std::cout << "Max Position: " << active_joint_bounds[i]->data()->max_position_
                  << "Min Position: " << active_joint_bounds[i]->data()->min_position_ <<std::endl;
        
        if (!active_joint_model_vector_[i]->satisfiesPositionBounds(state + active_joint_model_start_index_[i],
                                                                    *active_joint_bounds[i], margin)) 
        {
            std::cout << "Joint " << i << " is not satisfied" << std::endl;
            return false;
        }
    }
    return true;
}
  • Some times CHOMP finds the path (this is rare) and executes successfully, just like what running the demo.launch which uses ompl does. In most cases it shows the error Goal State violates joint limits. In some cases, it is not able to find the path after the maximum_iterations of chomp in the chomp_planning.yaml file. it outputs:

[ INFO] [1526507318.757404514]: iteration: 1980
[ INFO] [1526507318.783068634]: Forward kinematics took 0.024851328
[ INFO] [1526507318.810980139]: Forward kinematics took 0.025588620
[ INFO] [1526507318.839464849]: Forward kinematics took 0.025795736
[ INFO] [1526507318.867323901]: Forward kinematics took 0.025910782
[ INFO] [1526507318.895361434]: Forward kinematics took 0.025454463
[ INFO] [1526507318.923466469]: Forward kinematics took 0.025606107
[ INFO] [1526507318.951058233]: Forward kinematics took 0.025647301
[ INFO] [1526507318.978780636]: Forward kinematics took 0.025378841
[ INFO] [1526507319.007072810]: Forward kinematics took 0.025860659
[ INFO] [1526507319.035049796]: Forward kinematics took 0.025962257
[ INFO] [1526507319.037303818]: iteration: 1990
[ INFO] [1526507319.063960542]: Forward kinematics took 0.025763114
[ INFO] [1526507319.092076435]: Forward kinematics took 0.026019591
[ INFO] [1526507319.120225691]: Forward kinematics took 0.026079688
[ INFO] [1526507319.149020890]: Forward kinematics took 0.026431750
[ INFO] [1526507319.176720572]: Forward kinematics took 0.025346655
[ INFO] [1526507319.204480343]: Forward kinematics took 0.025782446
[ INFO] [1526507319.232124852]: Forward kinematics took 0.025243639
[ INFO] [1526507319.259777163]: Forward kinematics took 0.025283223
[ INFO] [1526507319.286742911]: Forward kinematics took 0.024933300
[ERROR] [1526507319.288709316]: Chomp path is not collision free!
[ INFO] [1526507319.288811040]: Terminated after 2000 iterations, using path from iteration 105
[ INFO] [1526507319.288838844]: Optimization core finished in 55.414118 sec
[ INFO] [1526507319.288872391]: Time per iteration 0.0277071
[ WARN] [1526507319.294813684]: Fail: ABORTED: No motion plan found. No execution attempted.

I will be trying some more things trying to fix this in the next couple of days. Will be trying the following:

  • A closer inspection of controllers.yaml , joint_limits.yaml , fake controller.yaml , yumi.srdf needs to be done and its linkages with the appropriate moveGroup, JointModel classes and looking up the chomp motion planner
  • using moveit_visual_tools to visualize the goal state to better understand why it might be invalid
  • Some of the code in moveIt_core might needs to be looked into for different function calls and some playing around with the code needs to be done to see what is being called and what might be breaking leading to providing us some hints as to what might be the problem in one of the yumi_move_it config / launch files.
@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 17, 2018

@bmagyar bmagyar referenced this issue May 17, 2018

Open

Chomp fix goalstate check #899

0 of 3 tasks complete
@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 17, 2018

great thanks @bmagyar for identifying the problem.

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 17, 2018

@raghavendersahdev please see #899 . The notes I put there addressing @Gene0213 could apply to you as well. I'd like to get your feedback whether it fixes the problem.

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 17, 2018

Thanks for fixing it @bmagyar , after incorporating your changes, Goal States violates Joint limits is gotten rid of.

I can confirm CHOMP works with yumi's left_arm but for some reason does not work for the right arm, earlier (before your PR #899) also while testing, I was occasionally only able to run CHOMP with the left arm. For you does it successfully plan for different start/end states for the right arm as well or is it just me doing something wrong.
For right arm (Planning Request / Planning Group = right_arm in Rviz), I get the following and the arm does not move:

[ INFO] [1526540946.069342321]: Setting joint yumi_joint_1_r to position 1.86522
[ INFO] [1526540946.069361869]: Setting joint yumi_joint_2_r to position -0.308871
[ INFO] [1526540946.069381350]: Setting joint yumi_joint_7_r to position -2.90483
[ INFO] [1526540946.069402018]: Setting joint yumi_joint_3_r to position -0.433432
[ INFO] [1526540946.069420475]: Setting joint yumi_joint_4_r to position -0.364844
[ INFO] [1526540946.069438412]: Setting joint yumi_joint_5_r to position 2.15176
[ INFO] [1526540946.069456407]: Setting joint yumi_joint_6_r to position -3.60309
[ INFO] [1526540946.070076349]: The following collision detectors are active in the planning scene.
[ INFO] [1526540946.070092826]: HYBRID
[ INFO] [1526540946.070109617]: Active collision detector is: HYBRID
[ INFO] [1526540946.070216987]: First coll check took 0.000084977
[ INFO] [1526540946.152135880]: Forward kinematics took 0.027157398
[ INFO] [1526540946.154304292]: iteration: 0
[ INFO] [1526540946.164637152]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[ INFO] [1526540946.164685253]: Chomp path is collision free
[ INFO] [1526540946.164801785]: Terminated after 1 iterations, using path from iteration -1
[ INFO] [1526540946.164835327]: Optimization core finished in 0.039880 sec
[ INFO] [1526540946.164852534]: Time per iteration 0.0398959
[ WARN] [1526540946.185472810]: Fail: ABORTED: Unknown event

For left arm (Planning Request / Planning Group = left_arm in Rviz) I get the following and the arm moves successfully:

[ INFO] [1526541012.169581401]: Setting joint yumi_joint_1_l to position -0.831145
[ INFO] [1526541012.169603597]: Setting joint yumi_joint_2_l to position -0.534639
[ INFO] [1526541012.169621045]: Setting joint yumi_joint_7_l to position -1.52168
[ INFO] [1526541012.169639765]: Setting joint yumi_joint_3_l to position -1.47903
[ INFO] [1526541012.169659404]: Setting joint yumi_joint_4_l to position 0.844988
[ INFO] [1526541012.169678243]: Setting joint yumi_joint_5_l to position -0.353595
[ INFO] [1526541012.169696250]: Setting joint yumi_joint_6_l to position -1.87642
[ INFO] [1526541012.170300987]: The following collision detectors are active in the planning scene.
[ INFO] [1526541012.170316263]: HYBRID
[ INFO] [1526541012.170328929]: Active collision detector is: HYBRID
[ INFO] [1526541012.284034013]: First coll check took 0.113666389
[ INFO] [1526541012.359676660]: Forward kinematics took 0.027466998
[ INFO] [1526541012.361757989]: iteration: 0
[ INFO] [1526541012.373303306]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[ INFO] [1526541012.373352081]: Chomp path is collision free
[ INFO] [1526541012.373479232]: Terminated after 1 iterations, using path from iteration -1
[ INFO] [1526541012.373513554]: Optimization core finished in 0.041326 sec
[ INFO] [1526541012.373529644]: Time per iteration 0.0413588
[ERROR] [1526541012.373875448]: Found empty JointState message

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 17, 2018

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 17, 2018

sure, I am testing with this yumi setup

@raghavendersahdev

This comment has been minimized.

Copy link
Contributor

raghavendersahdev commented May 17, 2018

For chomp I just cloned the moveIt git in my catkin_workspace and replaced this code for the file moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp manually from your PR

@ases200q2

This comment has been minimized.

Copy link

ases200q2 commented May 28, 2018

I got the same error ( Fail: ABORTED: Unknown event ) for a different robot. I think the source of the problem is here

for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
{
constraints_are_ok = constraints_are_ok and jc.configure(constraint);
constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied;
if (not constraints_are_ok)
{
res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
return false;
}
}

When I comment out this part, it works.

The other problem I get is that on Rviz, it works when first clicking "Plan" and then clicking "Execute" button but when clicking "plan and execute", I get "Start state is empty" error

@bmagyar

This comment has been minimized.

Copy link
Member

bmagyar commented May 28, 2018

Please don't copy snippets of code over but get a link from the repo that points there.
Yeah sure, if you comment out constraint checking it will not complain but you'll likely be left with broken planning results.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment