STOMP start point deviates from current robot state

[dieterwuytens]

I'm trying to use STOMP with this link . I want to plan a path with STOMP using a relative command for the endeffector. I'm trying to execute the following command to achieve this:

go z 0.1

I found this command in the help function, because it is not described in the tutorial. First time executing this command i get this:

[WARN] [1545329385.275367101]: JointLimits/endeffector Requested Start State is out of bounds
[ERROR] [1545329385.398778591]: Smoother, joint arm2_tele_joint polynomial fit failed!
[ERROR] [1545329385.398830444]: Unable to polynomial smooth trajectory!
[ERROR] [1545329385.398852533]: Updates filtering step failed

The path is still planned but when running the "go z 0.1" again, I get next error:

[ERROR] [1545329385.472031858]: 
Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'arm2_tele_joint': expected: 0, current: 0.166667

Why is this command not working with STOMP? I have updated the latest stomp packages. Thanks.

[jrgnicho]

Does the planning group you are planning for include the arm joints? Based on the first output line JointLimits/endeffector it looks as if it was planning for an endeffector group. The second error may not be related to the planner but more with the current state of your robot.

[dieterwuytens]

My planning group "endeffector" only includes a chain from base_link --> end_link. I thought that was the correct way, do i need to add the joints and the links to the planning group?

[jrgnicho]

I would start by seeing if the issue happens with another planner

[dieterwuytens]

I tested it on OMPL as well. Everything is working fine there, no errors. CHOMP gets an error too, but a diffrent one: start state is empty.

[jrgnicho]

Without more information It's hard to tell why it fails

[dieterwuytens]

the stomp_planning_pipeline.launch.xml file:

!-- Stomp Plugin for MoveIt! --> arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" !-- The request adapters (plugins) ORDER MATTERS -- arg name="planning_adapters" value="default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints" param name="planning_plugin" value="$(arg planning_plugin)" param name="request_adapters" value="$(arg planning_adapters)" param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" param name="trajectory_execution/allowed_start_tolerance" value="7.0"/> rosparam command="load" file="$(find coco_moveit_stomp_config)/config/stomp_planning.yaml"

the stomp_planning.yaml:

stomp/endeffector: group_name: endeffector optimization: num_timesteps: 60 num_iterations: 40 num_iterations_after_valid: 15 num_rollouts: 90 max_rollouts: 100 initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST, 4 : FILL_TRACJECTORY] control_cost_weight: 0.0 task: noise_generator: - class: stomp_moveit/GoalGuidedMultivariateGaussian stddev: [0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2] cost_functions: - class: stomp_moveit/CollisionCheck collision_penalty: 1.0 cost_weight: 1.0 kernel_window_percentage: 0.2 longest_valid_joint_move: 0.05 noisy_filters: - class: stomp_moveit/JointLimits lock_start: True lock_goal: True - class: stomp_moveit/MultiTrajectoryVisualization line_width: 0.02 rgb: [255, 255, 0] marker_array_topic: stomp_trajectories marker_namespace: noisy update_filters: - class: stomp_moveit/PolynomialSmoother poly_order: 6 - class: stomp_moveit/TrajectoryVisualization line_width: 0.05 rgb: [0, 191, 255] error_rgb: [255, 0, 0] publish_intermediate: True marker_topic: stomp_trajectory marker_namespace: optimized

If you need any other information, please say so.

[jrgnicho]

Thanks for providing this, at first glance it looks like you are missing the stomp_moveit/ToolGoalPose cost function and the stomp_moveit/ConstrainedCartesianGoal update_filter. See manipulator_rail configuration here Those plugins only work on the latest kinetic devel which is only available from source at the moment.

[dieterwuytens]

The first error is disappeared but the second error is still active:

[ERROR] [1545340616.731124047]: Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'arm2_tele_joint': expected: 0, current: 0.5805

I copied the "manipulator_rail" from the link that you provided and changed the planning group to endeffector. Also changed the initialization_method to 1 (LINEAR_INTERPOLATION)

My stomp_planning.yaml looks like this now:

stomp/endeffector: group_name: endeffector optimization: num_timesteps: 40 num_iterations: 40 num_iterations_after_valid: 0
num_rollouts: 10 max_rollouts: 10 initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST control_cost_weight: 0.0 task: noise_generator: - class: stomp_moveit/GoalGuidedMultivariateGaussian stddev: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] cost_functions: - class: stomp_moveit/CollisionCheck collision_penalty: 1.0 cost_weight: 1.0 kernel_window_percentage: 0.2 longest_valid_joint_move: 0.05 - class: stomp_moveit/ToolGoalPose position_cost_weight: 0.5 orientation_cost_weight: 0.5 noisy_filters: - class: stomp_moveit/JointLimits lock_start: True lock_goal: False - class: stomp_moveit/MultiTrajectoryVisualization line_width: 0.02 rgb: [255, 255, 0] marker_array_topic: stomp_trajectories marker_namespace: noisy update_filters: - class: stomp_moveit/ConstrainedCartesianGoal - class: stomp_moveit/PolynomialSmoother poly_order: 5 // (in comment) - class: stomp_moveit/ControlCostProjectionMatrix // - class: stomp_moveit/UpdateLogger // package: stomp_moveit // directory: log // filename: smoothed_update.txt - class: stomp_moveit/TrajectoryVisualization line_width: 0.05 rgb: [0, 191, 255] error_rgb: [255, 0, 0] publish_intermediate: True marker_topic: stomp_trajectory marker_namespace: optimized

[jrgnicho]

I think the problem you are having is related to this issue as the error message you are getting isn't coming from within stomp. It's possible that that command line tool is not grabbing the current robot state before calling the planner. So the planner is probably planning from a default position. In this case the trajectory execution will fail the the robot's current position isn't the default one.

[dieterwuytens]

Thanks for the link, but i'm new to this and I don't know how to use the following command.

move_group.setStartStateToCurrentState();

My python fuction for executing the planned path is the following:

def execute_zigzag(group_name):
	c = MoveGroupCommandInterpreter()

	# use endeffector
	(level, msg) = c.execute("use endeffector")
	print_message(level, msg)

	# rec c
	(level, msg) = c.execute("rec c")
	print_message(level, msg)
	# goal = c
	(level, msg) = c.execute("goal = c")
	print_message(level, msg)
	# omhoog bewegen
	(level, msg) = c.execute("goal[0] = 0")
	print_message(level, msg)
	(level, msg) = c.execute("goal[1] = 0")
	print_message(level, msg)
	(level, msg) = c.execute("goal[2] = 0.5805")
	print_message(level, msg)
	(level, msg) = c.execute("goal[3] = 0.5805")
	print_message(level, msg)
	(level, msg) = c.execute("goal[4] = 1.0")
	print_message(level, msg)
	(level, msg) = c.execute("goal[5] = 0")
	print_message(level, msg)
	(level, msg) = c.execute("goal[6] = 0")
	print_message(level, msg)
	(level, msg) = c.execute("goal[7] = 1.5707") #spuitkop verticaal
	print_message(level, msg)
	(level, msg) = c.execute("plan goal")
	print_message(level, msg)
	(level, msg) = c.execute("execute")
	print_message(level, msg)

	for x in range(1):
		# naar rechts bewegen
		execute_rel_motion(c, "x", "0.35")
		execute_rel_motion(c, "x", "0.35")

		# naar onder bewegen
		execute_rel_motion(c, "z", "-0.25")

		# naar links bewegen
		execute_rel_motion(c, "x", "-0.35")
		execute_rel_motion(c, "x", "-0.35")

		# naar links bewegen
		execute_rel_motion(c, "x", "-0.35")
		execute_rel_motion(c, "x", "-0.35")

		# naar onder bewegen
		execute_rel_motion(c, "z", "-0.25")

		# naar rechts bewegen
		execute_rel_motion(c, "x", "0.35")
		execute_rel_motion(c, "x", "0.35")

And the following two functions:

def kalibreer_spuitkop(c, msg):
    if (msg == "Failed while moving x") or (msg == "Failed while moving z"):
	print("Spuitkop bewegen weg van muur")
	(level, msg) = c.execute("go y -0.01")
	return
    var = 1
    while var:
	(level, msg) = c.execute("go y 0.01")
	if (msg == "Failed while moving y"):
		print("Kalibratie tussen spuitkop en muur compleet")
		var = 0

def execute_rel_motion(c, direction, dx):
    kalibreer_spuitkop(c, "")
    (level, msg) = c.execute("go " + direction + " " + dx)
    while (msg == "Failed while moving x") or (msg == "Failed while moving z"):
	kalibreer_spuitkop(c, msg)
	(level, msg) = c.execute("go " + direction + " " + dx)
    print_message(level, msg)

How do i combine the

move_group.setStartStateToCurrentState();

with my python file since i'm not using that.

[jrgnicho]

I think you might have to change the MoveGroupCommandInterpreter so that it fills the start start of the plan request with the actual current state. I'm not familiar with the MoveGroupCommandInterpreter so I wouldn't be able to tell you where that change should be made.

[ruinianxu]

@jrgnicho Hi Nicho, I am getting the following error right now. nvalid Trajectory: start point deviates from current robot state more than 0.01 joint 'joint_1': expected: -0.765456, current: -0.00030833 From the error message, it seems the joint 1 of the first point along planned trajectory deviates from the current joint 1 value. But I checked the computed trajectory and found the joint 1 value of the 1st point is exactly same as the current one. Meanwhile no matter how many times I tried, the expected values are same. Could you give me your understanding of this error message such that I can have a direction about how to solve this problem?

[jrgnicho]

Please post all stomp related questions in the new repo https://github.com/ros-industrial/stomp_ros

[gavanderhoorn]

@jrgnicho: I've transferred this issue to here (ie stomp_ros).

[akjay]

However, I met this problem recently days too.And after so many time tries,I found that, if you plan with rviz interactive, I mean, set a target by rviz interactive and then press plan (not Plan and Execute) button, you will get a correct plan, after plan has done ,you can press Execute button to move this trajectory.Once you want to do this two things together by set a target and then press Plan and Execute, the STOMP Planner will not work because the start state is been wrongly set to all 0.But if you do plan first and then execute, the planner will get start state correctly.Based on this, I tried to write program with c++ through the move_group interface to do more test.Firstly, I use move_group.move() function, it is the same way as PlanAndExecute, certainly,failed with the error message "start point deviates from current robot state".Secondly,I split it into two steps, one is move_group.plan(), and while plan finished, I call move_group.execute() to execute the trajectory,but this time still failed with the same error.I'm still trying to find out the difference between this two way.Anyone else use STOMP has this problem too?

[xibeisiber]

However, I met this problem recently days too.And after so many time tries,I found that, if you plan with rviz interactive, I mean, set a target by rviz interactive and then press plan (not Plan and Execute) button, you will get a correct plan, after plan has done ,you can press Execute button to move this trajectory.Once you want to do this two things together by set a target and then press Plan and Execute, the STOMP Planner will not work because the start state is been wrongly set to all 0.But if you do plan first and then execute, the planner will get start state correctly.Based on this, I tried to write program with c++ through the move_group interface to do more test.Firstly, I use move_group.move() function, it is the same way as PlanAndExecute, certainly,failed with the error message "start point deviates from current robot state".Secondly,I split it into two steps, one is move_group.plan(), and while plan finished, I call move_group.execute() to execute the trajectory,but this time still failed with the same error.I'm still trying to find out the difference between this two way.Anyone else use STOMP has this problem too?

I have the same problem. still don't how to fix this...

[runeg96]

@xibeisiber I had the same problem today when trying to plan using the python interface and STOMP but it worked perfectly in rviz. So i fixed it by setting the start state of the robot before planning :

state = robot.get_current_state()
group.set_start_state(state)