MTC Task Execution Failure - Moveit2 Pick n Place MTC demo about moveit2_tutorials

[sea-bass]

Thanks for reporting this! Note that I've just added the allow_nonzero_velocity_at_trajectory_end: true to the MoveIt Panda config, so that part at least should be okay going forward.

moveit/moveit_resources#198

[sea-bass]

I think I was able to reproduce this issue, have you recently checked out the MoveIt 2 tutorials?

As shown in moveit/moveit_task_constructor#540, I suspect there might have been some recent breaking changes in the moveit_task_constructor repo.

On your system could you:

1. Go to the src/moveit_task_constructor folder in your repo
2. Do git checkout ros2-old
3. Rebuild your workspace
4. Try again

And let us know if that resolves the issue?

[sea-bass]

And a different question -- when I run this demo on Rolling, the only "green" object that shows up is the cylinder, but not the larger cuboid at the bottom. Was this modification something you made, or are we running different versions of the tutorial? Wondering if this has any impact on the collision checks.

[trentatsa]

Hi @sea-bass , I have tested it out. Still the same issue. With regards to the extra shape I took a screenshot from another demo (moveit_task_contructor pick_place demo) but the issue is the same.

Based on moveit/moveit_task_constructor#540 I would say that the issue is based on ExecuteTaskSolutionCapability plugin. More specifically it seems like after the gripper is opened, ExecuteTaskSolutionCapability plugin is causing movegroup to not get the latest joint states of the gripper or hand and hence detects a false collision. Like I said if i make the initial joint position of the gripper open, the task execution is successful. You can replicate this by changing changing the contents of pand_hand.ros2_control.xacro to the following:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:macro name="panda_hand_ros2_control" params="name prefix">
        <ros2_control name="${name}" type="system">
            <hardware>
                <plugin>mock_components/GenericSystem</plugin>
            </hardware>
            <joint name="${prefix}panda_finger_joint1">
                <command_interface name="position" />
                <state_interface name="position">
                  <param name="initial_value">0.035</param>
                </state_interface>
                <state_interface name="velocity">
                    <param name="initial_value">0.0</param>
                </state_interface>
            </joint>
            <joint name="${prefix}panda_finger_joint2">
                <param name="mimic">${prefix}panda_finger_joint1</param>
                <param name="multiplier">1</param>
                <command_interface name="position" />
                <state_interface name="position">
                  <param name="initial_value">0.035</param>
                </state_interface>
                <state_interface name="velocity">
                    <param name="initial_value">0.0</param>
                </state_interface>
            </joint>
        </ros2_control>
    </xacro:macro>

</robot>

This will start the gripper at "open state" and from that point on no matter what happen to the state of the gripper, movegroup sees it as this position/state.

I think @henningkayser should be able to tell us whats going on here.

[rhaschke]

This might be related to moveit/moveit#3538, which I strongly recommended for porting to ROS2.

[rhaschke]

I've just added the allow_nonzero_velocity_at_trajectory_end: true

Nonzero velocities at the end of the trajectory are useful for servoing, but not for MTC: you want the robot to stop at a stage's end state. Otherwise end and new start will have different velocity vectors resulting in high jerk!

[sea-bass]

Nonzero velocities at the end of the trajectory are useful for servoing, but not for MTC: you want the robot to stop at a stage's end state. Otherwise end and new start will have different velocity vectors resulting in high jerk!

Yep, I had to add this in unrelated to this PR to address a Servo issue.

Unsure why this pick and place demo is trying to set nonzero end velocities though, as I agree all these planned motions should stop at rest.

[trentatsa]

@sea-bass @rhaschke so we dealing with two issues here:

1. non zero velocitys at end of trajectory.
2. MTC Task execution not getting getting updated planning scene using execution.

[rhaschke]

Unsure why this pick and place demo is trying to set nonzero end velocities though

Obviously, the time parameterization is broken.