Regression on the current kinetic-devel

aPonza commented 5 years ago

tl;dr 6ae03fac5cf19f830f60e10a5bf669e0148a8e1f doesn't fix for me the virtual joint issue that was fixed for #22, and it's not the case of just propagating the publisher to another launch file.

The issue

With the same exact process running, checking out panda_moveit_config 0.7.1 runs fine but the current kinetic-devel gives erratic behaviour and this diff in the output (I tried to keep it as short as possible:

--- /home/ap/Desktop/kinetic-devel
+++ /home/ap/Desktop/0.7.1
@@ -187,6 +187,7 @@
  * /position_joint_trajectory_controller/type: position_controll...
  * /robot_description: <?xml version="1....
  * /robot_description_kinematics/panda_arm/kinematics_solver: kdl_kinematics_pl...
+ * /robot_description_kinematics/panda_arm/kinematics_solver_attempts: 5
  * /robot_description_kinematics/panda_arm/kinematics_solver_search_resolution: 0.001
  * /robot_description_kinematics/panda_arm/kinematics_solver_timeout: 0.05
  * /robot_description_planning/joint_limits/panda_finger_joint1/has_acceleration_limits: False
@@ -229,6 +230,7 @@
  * /rosdistro: kinetic
  * /rosversion: 1.12.14
  * /rviz_ap_NUC7i7DNKE_15599_3838872138042140295/panda_arm/kinematics_solver: kdl_kinematics_pl...
+ * /rviz_ap_NUC7i7DNKE_15599_3838872138042140295/panda_arm/kinematics_solver_attempts: 5
  * /rviz_ap_NUC7i7DNKE_15599_3838872138042140295/panda_arm/kinematics_solver_search_resolution: 0.001
  * /rviz_ap_NUC7i7DNKE_15599_3838872138042140295/panda_arm/kinematics_solver_timeout: 0.05

@@ -288,13 +290,12 @@
 [ INFO] - [/move_group::PlanningSceneMonitor::startWorldGeometryMonitor::1025]: Starting world geometry monitor
 [ INFO] - [/move_group::PlanningSceneMonitor::startWorldGeometryMonitor::1041]: Listening to '/collision_object' using message notifier with target frame '/world '
 [ INFO] - [/move_group::PlanningSceneMonitor::startWorldGeometryMonitor::1056]: Listening to '/planning_scene_world' for planning scene world geometry
-[ INFO] - [/move_group::PointCloudOctomapUpdater::start::114]: Listening to '/camera/depth_registered/points' using message filter with target frame '/world '
+[ INFO] - [/move_group::PointCloudOctomapUpdater::start::114]: Listening to '/camera/depth_registered/points' using message filter with target frame '/panda_link0 '
 [INFO] - [/controller_spawner::main::192]: Controller Spawner: Loaded controllers: position_joint_trajectory_controller
 [ INFO] - [/move_group::PlanningSceneMonitor::startStateMonitor::1118]: Listening to '/attached_collision_object' for attached collision objects
 Context monitors started.
 [ INFO] - [/franka_control::FrankaHW::prepareSwitch::371]: FrankaHW: Prepared switching controllers to joint_position with parameters limit_rate=1, cutoff_frequency=100, internal_controller=joint_impedance
 [INFO] - [/controller_spawner::main::201]: Started controllers: position_joint_trajectory_controller
-[ERROR] - [/franka_control::main::206]: libfranka: Move command rejected: command not possible in the current mode!
 [ INFO] - [/move_group::OMPLInterface::OMPLInterface::55]: Initializing OMPL interface using ROS parameters
 [ INFO] - [/move_group::PlanningPipeline::configure::119]: Using planning interface 'OMPL'
 [ INFO] - [/move_group::FixWorkspaceBounds::FixWorkspaceBounds::53]: Param 'default_workspace_bounds' was not set. Using default value: 10
@@ -308,7 +309,6 @@
 [ INFO] - [/move_group::PlanningPipeline::configure::163]: Using planning request adapter 'Fix Start State Bounds'
 [ INFO] - [/move_group::PlanningPipeline::configure::163]: Using planning request adapter 'Fix Start State In Collision'
 [ INFO] - [/move_group::PlanningPipeline::configure::163]: Using planning request adapter 'Fix Start State Path Constraints'
-[ WARN] - [/move_group::CurrentStateMonitor::tfCallback::441]: Unable to update multi-DOF joint 'virtual_joint': TF has no common time between '/world' and 'panda_link0': 
 [ INFO] - [/franka_control::<lambda::131]: Recovered from error
 [ INFO] - [/move_group::MoveItSimpleControllerManager::MoveItSimpleControllerManager::145]: Added FollowJointTrajectory controller for position_joint_trajectory_controller
 [ INFO] - [/rviz_ap_NUC7i7DNKE_15599_3838872138042140295::RenderWindow* rviz::RenderSystem::makeRenderWindow::441]: Stereo is NOT SUPPORTED
@@ -352,7 +352,6 @@

 [ INFO] - [/process_manager::core::RobotModel::buildModel::93]: Loading robot model 'panda'...
 [ INFO] - [/process_manager::core::RobotModel::buildModel::93]: Loading robot model 'panda'...
-[ WARN] - [/move_group::PlanningSceneMonitor::updateSceneWithCurrentState::1255]: The complete state of the robot is not yet known.  Missing virtual_joint
 [ INFO] - [/process_manager::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::MoveGroupInterfaceImpl::178]: Ready to take commands for planning group panda_arm.
 [ INFO] - [/process_manager::Panda::Panda::403]: Waiting for a subscriber on topic /world_to_panda_link0.
 [ INFO] - [/static_transform_aggregator::StaticTransformAggregator::sendAllTransforms_::98]: Currently static broadcasting:
@@ -376,12 +375,12 @@
 [ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
 [ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
 [ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 5 states (2 start + 3 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 5 states (2 start + 3 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 5 states (2 start + 3 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::129]: ParallelPlan::solve(): Solution found by one or more threads in 0.013718 seconds
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: SimpleSetup: Path simplification took 0.005437 seconds and changed from 3 to 2 states
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::129]: ParallelPlan::solve(): Solution found by one or more threads in 0.012583 seconds
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: SimpleSetup: Path simplification took 0.004107 seconds and changed from 3 to 2 states
 [ INFO] - [/process_manager::planning_interface::MoveGroupInterface::Plan cobotica::Panda::createValidPlan_::1134]: Visualizing plan for current target.
 [ INFO] - [/move_group::MoveGroupExecuteTrajectoryAction::executePath::94]: Execution request received
 [rostopic-12] process has finished cleanly
@@ -419,60 +418,24 @@
 [ INFO] - [/process_manager::updateTf::42]: Updated world->lightbulb_base transform.
 [ INFO] - [/process_manager::CodedScene::addObjectToPlanningScene_::107]: Added lamp_1 to the planning scene.
 [ INFO] - [/move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly::163]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint2 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint4 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint2 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint4 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
 [ INFO] - [/move_group::ModelBasedPlanningContext::useConfig::335]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
 [ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
 [ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
 [ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
 [ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
-[ WARN] - [/move_group::ConstrainedGoalSampler::sampleUsingConstraintSampler::139]: More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide::236]: Constraint satisfied:: Joint name: 'panda_joint1', actual value: 0.000001, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide::236]: Constraint satisfied:: Joint name: 'panda_joint2', actual value: 1.570700, desired value: 1.570700, tolerance_above: 0.000100, tolerance_below: 0.000000
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide::236]: Constraint satisfied:: Joint name: 'panda_joint3', actual value: 0.000011, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide::236]: Constraint satisfied:: Joint name: 'panda_joint4', actual value: -0.069800, desired value: -0.069800, tolerance_above: 0.000100, tolerance_below: 0.000000
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide::236]: Constraint violated:: Joint name: 'panda_joint5', actual value: -2.228626, desired value: -nan, tolerance_above: 0.000100, tolerance_below: 0.000100
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide::236]: Constraint satisfied:: Joint name: 'panda_joint6', actual value: -0.000074, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::JointConstraint::decide::236]: Constraint satisfied:: Joint name: 'panda_joint7', actual value: -0.000034, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
-[ERROR] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::215]: RRTConnect: Unable to sample any valid states for goal tree
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 1 states (1 start + 0 goal)
-[ERROR] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::215]: RRTConnect: Unable to sample any valid states for goal tree
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 1 states (1 start + 0 goal)
-[ERROR] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::215]: RRTConnect: Unable to sample any valid states for goal tree
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 1 states (1 start + 0 goal)
-[ERROR] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::215]: RRTConnect: Unable to sample any valid states for goal tree
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 1 states (1 start + 0 goal)
-[ WARN] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::131]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.020978 seconds
-[ INFO] - [/move_group::ModelBasedPlanningContext::solve::593]: Unable to solve the planning problem
-[ WARN] - [/process_manager::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::plan::840]: Fail: ABORTED: No motion plan found. No execution attempted.
-[ INFO] - [/process_manager::planning_interface::MoveGroupInterface::Plan cobotica::Panda::createValidPlan_::1134]: Visualizing plan for current target: FAILED
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 4 states (2 start + 2 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 5 states (2 start + 3 goal)
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::129]: ParallelPlan::solve(): Solution found by one or more threads in 0.022040 seconds
+[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: SimpleSetup: Path simplification took 0.005353 seconds and changed from 3 to 2 states
+[ INFO] - [/process_manager::planning_interface::MoveGroupInterface::Plan cobotica::Panda::createValidPlan_::1134]: Visualizing plan for current target.
 [ INFO] - [/move_group::MoveGroupExecuteTrajectoryAction::executePath::94]: Execution request received
 [ INFO] - [/move_group::TrajectoryExecutionManager::executeThread::1304]: Completed trajectory execution with status SUCCEEDED ...
 [ INFO] - [/move_group::MoveGroupExecuteTrajectoryAction::executePath::118]: Execution completed: SUCCEEDED
 [ INFO] - [/process_manager::Panda::moveInternal_::1106]: Executing plan for current target.
 [ INFO] - [/move_group::MoveGroupMoveAction::executeMoveCallback_PlanOnly::163]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
-[ INFO] - [/move_group::FixStartStatePathConstraints::adaptAndPlan::73]: Path constraints not satisfied for start state...
-[ INFO] - [/move_group::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide::627]: Orientation constraint violated for link 'panda_link8'. Quaternion desired: 0.746576 -0.309240 0.544224 -0.225424, quaternion actual: 0.923960 -0.382490 0.000037 0.000034, error: x=1.144130, y=0.738413, z=0.488842, tolerance: x=0.001000, y=0.001000, z=3.142000
-[ INFO] - [/move_group::FixStartStatePathConstraints::adaptAndPlan::75]: Planning to path constraints...
-[ INFO] - [/move_group::ModelBasedPlanningContext::useConfig::335]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: RRTConnect: Starting planning with 1 states already in datastructure
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 5 states (2 start + 3 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 6 states (2 start + 4 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 5 states (2 start + 3 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::292]: RRTConnect: Created 5 states (3 start + 2 goal)
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::129]: ParallelPlan::solve(): Solution found by one or more threads in 0.063474 seconds
-[ INFO] - [/move_group::OMPLPlannerManager::OMPLPlannerManager::185]: SimpleSetup: Path simplification took 0.002666 seconds and changed from 3 to 2 states
-[ INFO] - [/move_group::FixStartStatePathConstraints::adaptAndPlan::92]: Planned to path constraints. Resuming original planning request.
 [ INFO] - [/move_group::core::RobotModel::buildModel::93]: Loading robot model 'panda'...
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint2 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint4 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint2 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
-[ WARN] - [/move_group::JointConstraint::configure::179]: Joint panda_joint4 is constrained to be above the maximum bounds. Assuming maximum bounds instead.
 [ INFO] - [/move_group::ModelBasedPlanningContext::useConfig::335]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
 [ INFO] - [/move_group::base::StateSamplerPtr ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler::200]: panda_arm: Allocating specialized state sampler for state space
 [ INFO] - [/move_group::base::StateSamplerPtr ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler::200]: panda_arm: Allocating specialized state sampler for state space

Basically what I see is kinetic-devel giving many more collision states and joint constraint violations without any physical differences between the two plannings.

What I tried

Skimming the commits that happened between the two tags (convenience link to comparison), I can see:

removal of kinematics_solver_attempts: should be irrelevant
modification of virtual joint: I think this might be the cause, as everything seems to stem from Unable to update multi-DOF joint 'virtual_joint': TF has no common time between '/world' and 'panda_link0' (which is incidentally different from this).

Seems to be this this problem but I tried to apply the fix to my own higher level launch file or directly to panda_control_moveit_rviz, and it doesn't solve the issue, whereas like the comment says if I change floating to fixed it does work.

EDIT1: The addition of the virtual_joint_publisher removes these two messages:

[ WARN] - [/move_group::CurrentStateMonitor::tfCallback::441]: Unable to update multi-DOF joint 'virtual_joint': TF has no common time between '/world' and 'panda_link0': 
[ WARN] - [/move_group::PlanningSceneMonitor::updateSceneWithCurrentState::1255]: The complete state of the robot is not yet known.  Missing virtual_joint

but produces no further change in behaviour: Insufficient states in sampleable goal region, Orientation constraint violated for link 'x', More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler., etc as before.

EDIT2:

roswtf doesn't help much as the comments are the same with floating and fixed virtual joint;
I have a static transform aggregator as per what was suggested by tfoote in an answer some time ago and I tried not using it in case that was the issue by cheating my way through (sub to both topics I usually wait for subscribers for, and manually static publishing of the other transform I need) but to no different outcome.

rhaschke commented 5 years ago

@aPonza, could you check, whether https://github.com/ros-planning/moveit_tutorials/pull/387 fixes this issue? If so, please close this issue.

aPonza commented 5 years ago

It doesn't seem to, the output is mainly the same. Just to double check, I had to clone moveit, moveit_resources and moveit_tutorials, stash my changes in panda_moveit_config (which wasn't modified) and rebuild the workspace to get this fix in, correct? I don't understand how a fix in a tutorial would affect the issue I'm seeing with the floating/fixed joint to be honest, but I'll have time to look a bit more into it in some days' time hopefully.

rhaschke commented 5 years ago

https://github.com/ros-planning/moveit_tutorials/pull/387 addressed an issue in the motion_planning_api_tutorial, which directly uses the C++ API of MoveIt, circumventing the ROS interface provided via the move_group node.

Please provide exact commands to reproduce your issue. You didn't even mention, which of the tutorials you tried.

aPonza commented 5 years ago

I'm not trying tutorials, I'm using my own nodes and launch file of a pick and place process. It's exactly the same roslaunch command with floating vs fixed. I'll make it reproducible, you're right. It'll take some time, I'm working on another thing at the moment.

rickstaa commented 2 years ago

@rhaschke Maybe we can close this issue since kinetic is EOL?

rhaschke commented 2 years ago

The virtual joint should be type fixed. This doesn't require a virtual joint broadcaster, which is not provided by the real robot.

moveit / panda_moveit_config

Regression on the current kinetic-devel #33

The issue

What I tried