No matter what start joint pose is used, the planner refuses to start.

[UltronDestroyer]

Result is this error no matter what starting joint pose:

[ERROR] [/move_group] [1491459052.767709473, 39.284000000]: STOMP Start joint pose is out of bounds
[ERROR] [/move_group] [1491459052.767820584, 39.284000000]: STOMP failed to get the start and goal positions

I've tried EVERY joint pose and even set everything to be "continuous" which SHOULD return "true" for all satisfiesBounds() checks. Yet still fails.

[jrgnicho]

Can you provide more info on your robot setup? Continuous joints may not be supported by moveit, the stomp planner checks for the validity of the planning request's start state by calling underlying MoveIt! functions.

[UltronDestroyer]

Right. Understood.

What robot state does it load? I have a robot model that's loading in gazebo and publishes some state using the joint state publisher controller. It is using ROS control as well. The URDF was on revolute and specified upper and lower limits, yet no matter what plan was used/orientation used, (since it's almost impossible to specify start state in gazebo without modifying urdf), the stomp planner would just say it is out of bounds.

The only way I can think this is possible is that my URDF is somehow wrong?

[jrgnicho]

I'd look at the URDF joint limits to make sure they're defined correctly. Also, how are you create the motion plan request that gets sent to the planner?

[UltronDestroyer]

The motion plan request is created using the RViz Motion planning plugin.

Do you see anything wrong with these urdf joint limits? (I'm guessing the rpy in the joint definition defines how it is positoined to start too?)

`

<xacro:property name="pi" value="3.14159" />

<xacro:property name="base_offsetx" value="-0.158825" />
<xacro:property name="base_offsety" value="-0.002" />
<xacro:property name="base_offsetz" value="0.407507" />

<xacro:macro name="arm">

<xacro:arm_link num="3" mat="black" par_num="2" axis="-1 0 0" offset="0.026 0 0" j_rpy="${pi/2} 0 ${3*pi/2}" l_rpy="0 ${-pi/2} ${-pi/2}" limit_low="-0.3" limit_high="1.6"/>

<xacro:arm_link num="4" mat="grey" par_num="3" axis="1 0 0" offset=" 0 0 -0.0855" j_rpy="0 ${pi/2} 0" l_rpy="${pi/2} ${pi} 0" limit_low="-3" limit_high="3"/>

<xacro:arm_link num="5" mat="black" par_num="4" axis="-1 0 0" offset="0.026 0 0" j_rpy=" ${-pi/2} 0 ${pi/2}" l_rpy="${-pi/2} 0 ${-pi/2}" limit_low="-1.5" limit_high="1.5"/>

<xacro:arm_link num="6" mat="red" par_num="5" axis="-1 0 0" offset="0 0 -0.0415" j_rpy="${-pi/2} ${-pi/2} 0" l_rpy="${pi/2} ${pi/2} 0" limit_low="-3" limit_high="3"/>

</xacro:macro>

<xacro:macro name="arm_link" params="num mat par_num axis offset j_rpy l_rpy limit_low limit_high ">

</xacro:macro>

`

[jrgnicho]

A quick glance shows nothing wrong with the limits. I'd run the display xacro launch file and with the gui enable to make sure that it is a valid xacro definition.

roslaunch urdf_tutorial xacrodisplay.launch model:=/path/to/my/robot.xacro gui:=true

[UltronDestroyer]

It works and they all look legit. Would having no collision elements defined affect anything?

[jrgnicho]

The lack of collision elements should not affect the joint check. I'm not sure how your setup combines gazebo and the rviz motion planning plugin, could you describe that in more detail?

[UltronDestroyer]

What files exactly are you looking for? I have a moveit configuration package that configures the stomp stuff. There are controllers spawned in gazebo via ros control for it, and a joint state publisher controller which publishes the joint states of the robot.

[jrgnicho]

I'm not looking for any files in particular , I'm just trying to determine if the start state is being properly initialized but it sounds like that isn't the issue here.

[UltronDestroyer]

So:

RViz motion planning plugin request -> MoveIT! (which is configured w/ the stomp setup) -> reads joint state -> returns fail. (No execution happens as it stops at failed plan). Joint states would be derived from the gazebo's joint state publisher controller defined here:

joint_state_publisher:
type: joint_state_controller/JointStateController
publish_rate: 70

[UltronDestroyer]

What I think im going to try is editing those "checks"(if else statement) out of the stomp_moveit.cpp plugin and see if it works just fine.

[jrgnicho]

The Rviz motion planning plugin doesn't automatically sync it's start state to the current state of the robot before sending a plan request to MoveIt!. You can view the start state in rviz by enabling it's visibility flag under the options in MotionPlanning

[UltronDestroyer]

I will try this out and see if that fixes things!

[jrgnicho]

@TheDash let me know if you made any progress here, thanks

[broesdecat]

Hi,

I just ran into the same issue, and might be able to provide some comparison code. I have been testing my configuration with a variety of APIs and it typically works fine, so I am quite sure the basic input is correct and the issue is in my rosnode code or a bug.

The following code exhibits the issue (but not always):

robot_state::RobotState start_state(*group.getCurrentState());
group.setStartStateToCurrentState();

group.setApproximateJointValueTarget(targetPose, "ee_link");

moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);
group.execute(my_plan);

The following code never exhibits the issue:

robot_state::RobotState start_state(*group.getCurrentState());
group.setStartStateToCurrentState();

moveit_msgs::MotionPlanRequest req;
req.group_name = "ee_link";
req.workspace_parameters.header.stamp = ros::Time::now();
moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
planningScene->setCurrentState(req.start_state);

const moveit::core::JointModelGroup *jointGroup = kinematic_state->getJointModelGroup("ee_link");
kinematic_state->setFromIK(jointGroup, targetPose);
moveit_msgs::Constraints goal1 = kinematic_constraints::constructGoalConstraints(*kinematic_state, jointGroup);
req.goal_constraints.push_back(goal1);

planning_interface::MotionPlanResponse res;
planningPipeline->generatePlan(planningScene, req, res);
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);

moveit::planning_interface::MoveGroup::Plan my_plan;
my_plan.start_state_ = response.trajectory_start;
my_plan.trajectory_ = response.trajectory;
group.execute(my_plan);

Best,
Broes

@jrgnicho: I came across the issue while looking for an industrial-moveit API that can plan to a cartesian position, ignoring orientation. setApproximateJointValueTarget was about my last guess, but doesn't seem to take setGoalOrientationTolerance into account. Is there any way to do this? (I should probably open a different feature-request for it)

[UltronDestroyer]

Thank you @broesdecat. I'll have to look into this another time. @jrgnicho I haven't had time to continue this project to provide feedback - but the jist is that I had the same error when i left off and was unable to solve it.

[jrgnicho]

@TheDash I'd be good if you could print the joint values of the start and goal states. Also, in theory the check bounds function on continuous joints should validate but it doesn't look like moveit takes that into account, see here

[jrgnicho]

@broesdecat is this a STOMP issue or a moveit issue? Base on your comments I can't tell which one it is.

[broesdecat]

@jrgnicho With OMPL it works fine, so my first guess would be STOMP. In attachment the code of my rosnode which gives issues with STOMP.

trajectory_node.txt

[jrgnicho]

@broesdecat could you point me to a repo that I could test this on?

[jrgnicho]

@broesdecat and @TheDash after further inspection using @broesdecat example code I pinned it down to the MoveGroupInterface::group.setStartStateToCurrentState() method, it in fact doesn't do anything other than to reset a smart pointer to null. I then modified Stomp to print the received start state and this is what was shown each time:

[ INFO]: Start State:
joint_state: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: 
  name[]
  position[]
  velocity[]
  effort[]
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: 
  joint_names[]
  transforms[]
  twist[]
  wrench[]
attached_collision_objects[]
is_diff: 1

As you can see the start state message is empty. I solved it by setting the MoveGroupInterface current state as the start state explicitly:

  robot_state::RobotState start_state(*group.getCurrentState());
  group.setStartState(start_state);

The state printout showed the state fully populated with joint values after that tweak

However you should refrain from using the MoveGroupInterface class in as much as you can as it is now deprecated. Thanks for reporting this issue.

[bbrito]

I got the same problem error but because in the starting state stomp also considers my joint from the wheels. How can I fix this?

I also set the joints as passive and it didn't work. Maybe the issue that I opened should be moved here.

[jrgnicho]

@ipa-bfb are you setting the robot state start state as described above?

[broesdecat]

@jrgnicho Confirmed, setting the startstate with the alternative API call solves the issue.

[jrgnicho]

@broesdecat Excellent. This probably merits an issue in the moveit repo to address this problem with the MoveGroupInterface.

[UltronDestroyer]

Interesting. That method may also be why if I relaunch things and try to plan, it never works. Because it loses its state and cannot re-find itself.

[janisa9]

Hi there!

Is this issue solved? I am having similar issue like @TheDash @broesdecat and others and in some other bugs #42. Everything is working fine with OMPL, but with STOMP I am getting these errors

[ WARN] [1523451873.799692191]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as start state in the motion planning request
[ WARN] [1523451873.811743347]: JointLimits/manipulator Requested Start State is out of bounds

[ERROR] [1523451873.899546947]: Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'elbow_joint': expected: 0, current: -1.745

I tried to set start state as @jrgnicho mentioned, but still nothing.

Also in STOMP planner I modified code to get start state and they are zeros (not even surprised). No wonder why start point deviates from current robot state

[ INFO] [1523451873.811849578]: Start_state_stomp =joint_state: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: 
  name[]
  position[]
  velocity[]
  effort[]
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: 
  joint_names[]
  transforms[]
  twist[]
  wrench[]
attached_collision_objects[]
is_diff: 1

In mine app I am giving start state like this

  moveit_msgs::RobotState robot_state;
  robot_state::RobotStatePtr current_state= move_group_ptr->getCurrentState();
  robot_state::robotStateToRobotStateMsg(*current_state,robot_state);
  ROS_INFO_STREAM("mt_start ="<< robot_state);
  move_group_ptr->setStartState(robot_state);

And getting this start state in print, as it should be. But it doesn't get to STOMP planner..

[ INFO] [1523451873.599566010]: mystart =joint_state: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: /base_link
  name[]
    name[0]: shoulder_pan_joint
    name[1]: shoulder_lift_joint
    name[2]: elbow_joint
    name[3]: wrist_1_joint
    name[4]: wrist_2_joint
    name[5]: wrist_3_joint
  position[]
    position[0]: -0.5
    position[1]: -0.87
    position[2]: -1.745
    position[3]: -2.09
    position[4]: 1.57
    position[5]: 1.57
  velocity[]
  effort[]
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 0.000000000
    frame_id: /base_link
  joint_names[]
  transforms[]
  twist[]
  wrench[]
attached_collision_objects[]
is_diff: 0

Maybe i have missed something, and i don't even know anymore what is wrong start state or current state because from this STOMP error -->
[ERROR] [1523451873.899546947]: Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'elbow_joint': expected: 0, current: -1.745

One more time wanted to say that everything works with OMPL motion planner. And even with UR5_moveit_config demo.launch everything is fine and i can plan motions using RVIZ motionplanning plugin and STOMP motion planner.

It really makes me wonder in which side is problem, my app, moveit, or STOMP? btw using ROS-Kinetic

[janisa9]

Finally solved this, by going though RViz Motion planning plugin, and searching how movements are executed in there, because this method was working for me.

So basically as mentioned in #53 STOMP failes to plan with using the move_group move() method

to solve this I needed to use
move_group_ptr->execute(my_plan)

instead of

move_group_ptr->move()

I am wondering why i couldn't find any information about this and if anyone has similar problems today.

[jrgnicho]

related to #53

[rickstaa]

This issue also occurs when working with the RVIZ MotionPlanning plugin. This is expected as it also uses the ->execute(my_plan) method (see https://github.com/ros-planning/moveit/blob/865c2611285aae9c6f733a1c40f87e5b5f4ba1cc/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp#L203).