Open trentatsa opened 8 months ago
sea-bass
commented
8 months ago 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.
sea-bass
commented
8 months ago I think I was able to reproduce this issue, have you recently checked out the MoveIt 2 tutorials?
As shown in https://github.com/ros-planning/moveit_task_constructor/issues/540, I suspect there might have been some recent breaking changes in the moveit_task_constructor repo.
On your system could you:
src/moveit_task_constructor folder in your repogit checkout ros2-oldAnd let us know if that resolves the issue?
sea-bass
commented
8 months ago 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
commented
8 months ago 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 https://github.com/ros-planning/moveit_task_constructor/issues/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
commented
8 months ago This might be related to https://github.com/ros-planning/moveit/pull/3538, which I strongly recommended for porting to ROS2.
rhaschke
commented
8 months ago 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
commented
8 months ago 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
commented
8 months ago @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
commented
8 months ago Unsure why this pick and place demo is trying to set nonzero end velocities though
Obviously, the time parameterization is broken.
Description
When running the moveit2_tutorials pick and place demo, the plan succeeds however the execution fails. The reason behind this is detection of collision 'object' and 'panda_leftfinger' duing the " approach object" trajectory execution. The reason behind this is that after the gripper opens in the demo, its seems like the trajectory execution does not use the latest planning scene i.e. it uses the planning scene when the gripper is still closed on startup. If I set the initail postion of the gripper to be open the demo pick and place runs successfully i.e. during the "approach object" and "retreat after place" the panda_leftfinger and panda_rightfinger are in the open position as seen by the trajectory execution. Im suspicious that it may be something to do with the ExecuteTaskSolutionCapability of move_group.
Your environment
Steps to reproduce
add allow_nonzero_velocity_at_trajectory_end: true to the ros_controllers.yaml of moveit_resources_panda_moveit_config so that it as below. This is done otherwise you get the error:
This is done otherwise you get the following error in control:
Run the following: ros2 launch moveit2_tutorials mtc_demo.launch.py
and then run: ros2 launch moveit2_tutorials pick_place_demo.launch.py
Execute any of the solutions:
Expected behaviour
The robot should execute the pick and place :)
Actual behaviour
The task execution fails on " approach object" task component due to collision detection.
Backtrace or Console output