MoveIt! + Gazebo - Can't set a new start state

[znbrito]

Hello, I am trying to use MoveIt! with Gazebo to control the Robotis Manipulator-H arm. My devel is Kinetic. I added a comment on moveit/moveit#661, saying that my current state is not being refreshed and @v4hn answered to me, saying that move_group does not connect to the correct joint_states topic, and he was right and that problem is solved. My problem now is that whenever I execute a plan and after that try try to set a new start state, it is not properly assigned and my program always assumes that my start state to any plan is my first state. This is really strange because I can set a new goal pose from my current state after I move my robot but I can't set a new start state, any solutions? I tried to use:

moveit::core::RobotState new_start_state = (*move_group.getCurrentState());
move_group.setStartState(new_start_state);

OR

move_group.setStartStateToCurrentState();

But none of them worked.

Btw, the code in this repository is not my last version.
Thanks in advance!

[v4hn]

move_group.setStartStateToCurrentState();

Unless you know what you are doing, always go with this version. It's the same as not specifying anything about the start state.

[znbrito]

@v4hn yes, it was the first thing I tried but it is not working. It seems like in the current version of that function, the start state is reset instead of being updated... any solution?

[v4hn]

It seems like in the current version of that function, the start state is reset instead of being updated.

This is the intended behavior, as discussed at length in the corresponding issue/pr at moveit. An empty/unset start state in the `MoveGroupInterface` means the `move_group` node will fetch the current state when it is asked to plan a motion. If you need the current state of the robot in your own node, you should instantiate a `PlanningSceneMonitor`/`CurrentStateMonitor` yourself. I don't know your current problem, but if you do not set the start state of the MGI, then it is somewhere else.

[znbrito]

@v4hn I have seen some discussion about the planning scene monitor in the following websites:

moveit/moveit_ros#442
https://groups.google.com/forum/#!topic/moveit-users/ZYsWL1jRWyY

But I just can't seem to implement it, I always get strange errors that I can't identify. However, the "planning_scene_monitor::CurrentStateMonitor Class Reference" seems to be a good solution, the only problem is that I don't know how to implement it, I am a newbie in ROS and in MoveIt! and for now I am just following the Move Group tutorials for Kinetic and trying to integrate all of it with Gazebo. I created a branch named "Error" in this repository, for you to see my current error. Right now I am just trying to do the following: move the arm to a goal pose and then, starting from that pose goal, move the arm to a joint space goal (which in fact just means to move a single joint). The problem is that the robot keeps returning to the "initial" position of the simulation before heading to the joint space goal that I defined. I tried to set a new start state in 2 different ways, as you can see in my above comment, but it didn't affect anything.

To run the simulation, open 3 terminals and run the following:

first terminal: roslaunch manipulator_h_gazebo manipulator_h_gazebo.launch
second terminal: roslaunch manipulator_h_moveit_config manipulator_h_planning_execution.launch
third terminal: roslaunch manipulator_h_path_planning manipulator_h_move_group_interface_tutorial.launch

You just need to click "Next" on the "RvizVisualToolsGui" to go through the simulation.

The only file that I wrote/adpated is the "manipulator_h_move_group_interface_tutorial.cpp" file, which is inside the "manipulator_h_path_planning" package. This file is adapted from the PR2 example in the move group tutorials for MoveIt!.

I know that I don't understand much but if you could take a look, it would be great! :)
Thanks in advance.

[znbrito]

@v4hn new update. I think my error may have something to do with my moveit configuration. If you run the following in 2 terminals:

$ roslaunch manipulator_h_gazebo manipulator_h_gazebo.launch
$ roslaunch manipulator_h_moveit_config manipulator_h_planning_execution.launch

You see in Gazebo that if you first execute a plan in RViz and move the arm to a random position, it is not possible to execute another RViz plan only by changing the goal state, because the start state is not being updated and the robot will always first go to the initial state (the start state of the first executed plan) before he heads to the final state of the second plan. That is strange no?

I tryed to do the same for the UR5 manipulator. I followed this steps:

1 - First I Installed the following packages to simulate the arm (ur5) in gazebo:

$ sudo apt-get install ros-kinetic-ur-gazebo
$ sudo apt-get install ros-kinetic-ur5-moveit-config
$ sudo apt-get install ros-kinetic-ur-kinematics

2 - To launch the simulated arm and a controller for it, run:

$ roslaunch ur_gazebo ur5.launch

and in another terminal:

$ roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true

To control the simulated arm from RViz, also run in another terminal:

$ roslaunch ur5_moveit_config moveit_rviz.launch config:=true

Now try to execute 2 plans in RViz and the start state of the second plan will be the goal state of the first plan, which is the normal behaviour that I was expecting, which also means that the start state is being updated. So why is that not happening with my manipulator-h configuration?

Many thanks!

[v4hn]

I'm sorry, but I don't have the time to look into your specific problem.
It still sounds like you did not properly connect your joint_states topics though.

[znbrito]

@v4hn you are right, I was missing the brackets "[" and "]" when I define the topic name. Thanks for your help and sorry to disturb!