search

sangteak601 / mujoco_ros2_control_examples

mujoco_ros2_control_examples
MIT License
0 stars 0 forks source link

Invalid Start State #1

Open dyackzan opened 1 week ago

dyackzan commented 1 week ago

When I start things up it looks like the Panda is in collision with itself

Screenshot from 2024-06-26 08-04-20

And I see this in the log when I attempt to run "Plan and Execute" from RViz

[move_group-4] [INFO] [1719410610.248029404] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-4] [INFO] [1719410610.248185176] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[rviz2-1] [INFO] [1719410610.248411540] [move_group_interface]: Plan and Execute request accepted
[move_group-4] [WARN] [1719410610.348279306] [moveit_ros.current_state_monitor]: No state update received within 100ms of system clock. Have been waiting for 0.000000s, timeout is 1.000000s
[move_group-4] [INFO] [1719410611.249266008] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 42.208000, but latest received state has time 0.000000.
[move_group-4] Check clock synchronization if your are running ROS across multiple machines!
[move_group-4] [WARN] [1719410611.249318195] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to fetch current robot state.
[move_group-4] [INFO] [1719410611.249411225] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-4] [INFO] [1719410611.249453934] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-4] [INFO] [1719410611.249462876] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'ompl'
[move_group-4] [INFO] [1719410611.249671428] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'panda_link5' (type 'Robot link') and 'panda_link7' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-4] [INFO] [1719410611.249683466] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-4] [INFO] [1719410611.249691882] [moveit_ros.fix_start_state_collision]: Start state appears to be in collision with respect to group panda_arm
[move_group-4] [WARN] [1719410611.260290845] [moveit_ros.fix_start_state_collision]: Unable to find a valid state nearby the start state (using jiggle fraction of 0.020000 and 100 sampling attempts). Passing the original planning request to the planner.
[move_group-4] [INFO] [1719410611.260391085] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-4] [WARN] [1719410611.260539965] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [ERROR] [1719410611.260572071] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [WARN] [1719410611.260578721] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [WARN] [1719410611.260607019] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [ERROR] [1719410611.260627080] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [WARN] [1719410611.260696542] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [ERROR] [1719410611.260717850] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [ERROR] [1719410611.260734569] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [WARN] [1719410611.260821382] [ompl]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000353 seconds
[move_group-4] [WARN] [1719410611.260907762] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [WARN] [1719410611.260919580] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [WARN] [1719410611.260936468] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [ERROR] [1719410611.260954950] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [ERROR] [1719410611.260964151] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [ERROR] [1719410611.260975833] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [WARN] [1719410611.261072529] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [ERROR] [1719410611.261095040] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [WARN] [1719410611.261127468] [ompl]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000279 seconds
[move_group-4] [WARN] [1719410611.261162545] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [ERROR] [1719410611.261177090] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [WARN] [1719410611.261186043] [ompl]: ./src/ompl/base/src/Planner.cpp:248 - panda_arm/panda_arm: Skipping invalid start state (invalid state)
[move_group-4] [ERROR] [1719410611.261200373] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - panda_arm/panda_arm: Motion planning start tree could not be initialized!
[move_group-4] [WARN] [1719410611.261214165] [ompl]: ./src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000076 seconds
[move_group-4] [INFO] [1719410611.279877031] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-4] [INFO] [1719410611.279889445] [moveit_move_group_default_capabilities.move_action_capability]: Catastrophic failure
[rviz2-1] [INFO] [1719410611.280214990] [move_group_interface]: Plan and Execute request aborted
[rviz2-1] [ERROR] [1719410611.280273936] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached
sangteak601 commented 1 week ago

I think I experienced this before when I was using MJCF with different joint names from URDF . Did you clone mujoco menagerie from mine? Could you please check if controllers are initialized properly from the log? You should see something like this. 

[spawner-7] [INFO] [1719419342.805734640] [spawner_panda_arm_controller]: Configured and activated panda_arm_controller
[spawner-8] [INFO] [1719419342.824804701] [spawner_panda_hand_controller]: Configured and activated panda_hand_controller