Plan in rviz is really weird

[Joris Beuls]

Dear community,

I planned a path by setting joint states which i found by reading the "joint state publisher" UI from RViz. Now when i plan the path my robot does some weird rotations. This is shown in the video i added. Can i add maximum allowed rotations of a joint somewhere?
When i set a start and goal state in rviz itself and not in my C++ program the path is pretty good.

How can i optimize the path it finds?

A second question is: how can i visualize the attached object during movement and the resulting position? The shape is of type shape_msgs::SolidPrimitive (BOX).

Part of my code:

   

    std::vector<double> group_variable_values1;
    group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values1);

    group_variable_values1[0] = 0;
    group_variable_values1[1] = -0.42;
    group_variable_values1[2] = -1.36;
    group_variable_values1[3] = 3.16;
    group_variable_values1[4] = 1.32;
    group_variable_values1[5] = 1.63;
    group.setJointValueTarget(group_variable_values1);

    success = group.plan(my_plan);

    ROS_INFO("Visualizing plan 1 (joint space goal) %s",success?"":"FAILED");

    /* Sleep to give Rviz time to visualize the plan. */
    sleep(5.0);

    group.execute(my_plan);

    /* set the last position as the new start position

    robot_state::RobotState start_state(*group.getCurrentState());
    const robot_state::JointModelGroup *joint_model_group =
                    start_state.getJointModelGroup(group.getName());
    start_state.setJointGroupPositions(joint_model_group, group_variable_values1);
    group.setStartState(start_state);

    /* Now, let's attach the collision object to the robot. */

    ROS_INFO("Attach the object to the robot");
    group.attachObject("box2");

    /* Sleep to give Rviz time to show the object attached (different color). */

    sleep(4.0);

    // Planning to a joint-space goal
    // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    //
    // Second position
    // First get the current set of joint values for the group.

    std::vector<double> group_variable_values2;
    group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values2);

    /* Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.*/

    group_variable_values2[0] = -1.54;
    group_variable_values2[1] = -0.46;
    group_variable_values2[2] = -1.97;
    group_variable_values2[3] = -3.14;
    group_variable_values2[4] = 0.73;
    group_variable_values2[5] = -1.54;
    group.setJointValueTarget(group_variable_values2);

    success = group.plan(my_plan);

    ROS_INFO("Visualizing plan 1 (joint space goal) %s",success?"":"FAILED");

    /* Sleep to give Rviz time to visualize the plan. */
    sleep(5.0);

    group.execute(my_plan);

    /* Now, let's detach the collision object to the robot. */

    ROS_INFO("Detach the object from the robot");
    group.detachObject("box2");

    /* Sleep to give Rviz time to show the object detached. */

    sleep(4.0);

Kind regards
Joris

[Simon Schmeißer]

Hi Joris,

for me it only works if I use move() instead of execute(). This of course then does a new planning. plan and execute seem to work with an outdated copy of the robotState, where the attaching has not happened yet. setStartStateToCurrentState() looks like a remedy but actually improves changes of this going wrong.

My impression is that movements in MoveIt! generally seem to be rather funny, but yours is impressive. This is at least partly due to the use of sampling based planners, but then I would expect even them to perform better. Maybe you are lucky with using a different planner? Also reducing the mobility does tend to help, ie once I modelled the actual robot cell and limited joint_1 to +-90deg things improved a lot. This is propably due to the greatly reduced state space were the samples can be taken from.

I was about to write a very similar mail, so I'm looking forward to more responses :)
Simon

[Joris Beuls]

Hello Simon,

I tried move() instead of execute() now i have the following differences between my simulations:

1) In the execute() simulation the box is added to the end effector but i don't see it move. In the move() simulation it is added but now i can see it move.
2) In the move() simulation the setting of a new start state doesn't work anymore, therefore the attached box floats in the air when i plan a new path.

Do you perhaps know what could cause this difference?

At last i have one more question. As you can see in the movie i posted previously the planned path is shown and executed. However, the robot does not seem to move and stays in the home position. Do you know a way to resolve this or is it normal in rviz?

kind regards
Joris

Op dinsdag 14 april 2015 18:00:20 UTC+2 schreef Simon Schmeißer:

[Joris Beuls]

Hello,

Apparently i was using a bad URDF file. Now everything works as expected. The plans are sometimes still weird but that is because of the planning algoritms.

Thanks for the answer you provided.

Best regards
Joris

Op maandag 20 april 2015 16:23:27 UTC+2 schreef Joris Beuls: