search

PilzDE / pilz_robots

PILZ robot manipulator module PRBT 6 in ROS
https://wiki.ros.org/pilz_robots
52 stars 25 forks source link

Robot cannot be moved after Emergency-Stop/Panic button/ Enabling button release #92

Closed hslusarek closed 5 years ago

hslusarek commented 5 years ago

Commit (Stand: 19.03.2019):

Steps to reproduce

Start real robot via:
roslaunch prbt_moveit_config moveit_planning_execution.launch sim:=false pipeline:=pilz_command_planner

  1. Perform motion (e.g. vial Python script)
  2. During motion do one of the following things:
    • release Enabling button,
    • bush panic button or
    • push emergency button.
  3. If no problem occures go back to step 1.

Expected behavior

After I do one of the following things it should be possible to continue to move the robot:

Observed behavior

It is not possible to continue to move the robot because on the console it is stated that: PATH_TOLERANCE_VIOLATED

hslusarek commented 5 years ago
jschleicher commented 5 years ago

Maybe duplicate of #93?

hslusarek commented 5 years ago

Bug was reproduced with robot with new firmeware and kinetic-devel

Version details

Console output

SUMMARY
========

CLEAR PARAMETERS
 * /prbt/driver/

PARAMETERS
 * /controller_joint_names: ['prbt_joint_1', ...
 * /move_group/allow_trajectory_execution: True
 * /move_group/capabilities: pilz_trajectory_g...
 * /move_group/controller_list: [{'action_ns': 'f...
 * /move_group/disable_capabilities: 
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_range: 5.0
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/octomap_resolution: 0.025
 * /move_group/planning_plugin: pilz::CommandPlanner
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: 
 * /move_group/start_state_max_bounds_error: 0.1
 * /move_group/trajectory_execution/allowed_execution_duration_scaling: 1.2
 * /move_group/trajectory_execution/allowed_goal_duration_margin: 0.5
 * /move_group/trajectory_execution/allowed_start_tolerance: 0.01
 * /pilz_modbus_read_client_node/index_of_first_register_to_read: 512
 * /pilz_modbus_read_client_node/modbus_server_ip: 169.254.60.1
 * /pilz_modbus_read_client_node/modbus_server_port: 502
 * /pilz_modbus_read_client_node/num_registers_to_read: 2
 * /prbt/driver/bus/device: can0
 * /prbt/driver/defaults/eds_file: config/prbt_0_1.dcf
 * /prbt/driver/defaults/eds_pkg: prbt_support
 * /prbt/driver/name: manipulator
 * /prbt/driver/nodes/prbt_joint_1/id: 3
 * /prbt/driver/nodes/prbt_joint_2/id: 4
 * /prbt/driver/nodes/prbt_joint_3/id: 5
 * /prbt/driver/nodes/prbt_joint_4/id: 6
 * /prbt/driver/nodes/prbt_joint_5/id: 7
 * /prbt/driver/nodes/prbt_joint_6/id: 8
 * /prbt/driver/sync/interval_ms: 10
 * /prbt/driver/sync/overflow: 0
 * /prbt/joint_names: ['prbt_joint_1', ...
 * /prbt/manipulator_joint_state_controller/publish_rate: 50
 * /prbt/manipulator_joint_state_controller/type: joint_state_contr...
 * /prbt/manipulator_joint_trajectory_controller/action_monitor_rate: 10
 * /prbt/manipulator_joint_trajectory_controller/constraints/goal_time: 0.6
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_1/goal: 0.01
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_1/trajectory: 0.157
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_2/goal: 0.01
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_2/trajectory: 0.157
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_3/goal: 0.01
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_3/trajectory: 0.157
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_4/goal: 0.01
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_4/trajectory: 0.157
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_5/goal: 0.01
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_5/trajectory: 0.157
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_6/goal: 0.01
 * /prbt/manipulator_joint_trajectory_controller/constraints/prbt_joint_6/trajectory: 0.157
 * /prbt/manipulator_joint_trajectory_controller/constraints/stopped_velocity_tolerance: 0.05
 * /prbt/manipulator_joint_trajectory_controller/joints: ['prbt_joint_1', ...
 * /prbt/manipulator_joint_trajectory_controller/required_drive_mode: 7
 * /prbt/manipulator_joint_trajectory_controller/state_publish_rate: 25
 * /prbt/manipulator_joint_trajectory_controller/stop_trajectory_duration: 0.2
 * /prbt/manipulator_joint_trajectory_controller/type: position_controll...
 * /prbt/max_command_silence: 0.5
 * /prbt/sto_modbus_adapter_node/index_of_first_register_to_read: 512
 * /robot_description: <?xml version="1....
 * /robot_description_kinematics/manipulator/kinematics_solver: prbt_manipulator_...
 * /robot_description_kinematics/manipulator/kinematics_solver_attempts: 3
 * /robot_description_kinematics/manipulator/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/manipulator/kinematics_solver_timeout: 0.005
 * /robot_description_planning/cartesian_limits/max_rot_vel: 1.57
 * /robot_description_planning/cartesian_limits/max_trans_acc: 2.25
 * /robot_description_planning/cartesian_limits/max_trans_dec: -5
 * /robot_description_planning/cartesian_limits/max_trans_vel: 1
 * /robot_description_planning/joint_limits/prbt_joint_1/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_1/max_acceleration: 3.49
 * /robot_description_planning/joint_limits/prbt_joint_1/max_velocity: 1.57
 * /robot_description_planning/joint_limits/prbt_joint_2/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_2/max_acceleration: 3.49
 * /robot_description_planning/joint_limits/prbt_joint_2/max_velocity: 1.57
 * /robot_description_planning/joint_limits/prbt_joint_3/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_3/max_acceleration: 3.49
 * /robot_description_planning/joint_limits/prbt_joint_3/max_velocity: 1.57
 * /robot_description_planning/joint_limits/prbt_joint_4/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_4/max_acceleration: 3.49
 * /robot_description_planning/joint_limits/prbt_joint_4/max_velocity: 1.57
 * /robot_description_planning/joint_limits/prbt_joint_5/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_5/max_acceleration: 3.49
 * /robot_description_planning/joint_limits/prbt_joint_5/max_velocity: 1.57
 * /robot_description_planning/joint_limits/prbt_joint_6/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_6/has_velocity_limits: True
 * /robot_description_planning/joint_limits/prbt_joint_6/max_acceleration: 3.49
 * /robot_description_planning/joint_limits/prbt_joint_6/max_velocity: 1.57
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /rviz_ad_cms_titan_10963_3108490594134928558/manipulator/kinematics_solver: prbt_manipulator_...
 * /rviz_ad_cms_titan_10963_3108490594134928558/manipulator/kinematics_solver_attempts: 3
 * /rviz_ad_cms_titan_10963_3108490594134928558/manipulator/kinematics_solver_search_resolution: 0.005
 * /rviz_ad_cms_titan_10963_3108490594134928558/manipulator/kinematics_solver_timeout: 0.005

NODES
  /prbt/
    controller_spawner (controller_manager/spawner)
    driver (canopen_motor_node/canopen_motor_node)
    joint_state_relay (topic_tools/relay)
    sto_modbus_adapter_node (prbt_hardware_support/sto_modbus_adapter_node)
  /
    move_group (moveit_ros_move_group/move_group)
    pilz_modbus_read_client_node (prbt_hardware_support/pilz_modbus_read_client_node)
    robot_init (rosservice/rosservice)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz_ad_cms_titan_10963_3108490594134928558 (rviz/rviz)

auto-starting new master
process[master]: started with pid [10985]
ROS_MASTER_URI=http://localhost:11008/

setting /run_id to 6e64d6b6-7878-11e9-a4b9-509a4c59ef48
process[rosout-1]: started with pid [10998]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [11016]
process[prbt/joint_state_relay-3]: started with pid [11017]
process[prbt/driver-4]: started with pid [11018]
process[prbt/controller_spawner-5]: started with pid [11034]
process[robot_init-6]: started with pid [11046]
process[pilz_modbus_read_client_node-7]: started with pid [11049]
process[prbt/sto_modbus_adapter_node-8]: started with pid [11059]
process[move_group-9]: started with pid [11068]
process[rviz_ad_cms_titan_10963_3108490594134928558-10]: started with pid [11094]
[ INFO] [1558079427.077973151]: waitForService: Service [/prbt/manipulator_joint_trajectory_controller/hold] has not been advertised, waiting...
[ INFO] [1558079427.158681006]: Loading robot model 'prbt'...
[ WARN] [1558079427.158768436]: Skipping virtual joint 'FixedBase' because its child frame 'prbt_base_link' does not match the URDF frame 'world'
[ INFO] [1558079427.158789684]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1558079427.179164146]: rviz version 1.12.17
[ INFO] [1558079427.179200406]: compiled against Qt version 5.5.1
[ INFO] [1558079427.179209976]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1558079427.245088485]: Using fixed control period: 0.010000000
[ INFO] [1558079427.312702471]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1558079427.315009231]: MoveGroup debug mode is OFF
Starting context monitors...
[ INFO] [1558079427.315038271]: Starting scene monitor
[ INFO] [1558079427.317399786]: Listening to '/planning_scene'
[ INFO] [1558079427.317418121]: Starting world geometry monitor
[ INFO] [1558079427.319915469]: Listening to '/collision_object' using message notifier with target frame '/world '
[ INFO] [1558079427.322597903]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1558079427.334546484]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1558079427.339900776]: Reading limits from namespace /robot_description_planning
[ INFO] [1558079427.345674753]: Initializing XXX
[ INFO] [1558079427.345824512]: Current state: 1 device error: system:0 internal_error: 0 (OK)
[ INFO] [1558079427.345912592]: Current state: 2 device error: system:0 internal_error: 0 (OK)
[ INFO] [1558079427.368558006]: Available plugins: pilz::PlanningContextLoaderCIRC pilz::PlanningContextLoaderLIN pilz::PlanningContextLoaderPTP 
[ INFO] [1558079427.368584032]: About to load: pilz::PlanningContextLoaderCIRC
[ INFO] [1558079427.370527321]: Registered Algorithm [CIRC]
[ INFO] [1558079427.370565248]: About to load: pilz::PlanningContextLoaderLIN
[ INFO] [1558079427.371823503]: Registered Algorithm [LIN]
[ INFO] [1558079427.371840168]: About to load: pilz::PlanningContextLoaderPTP
[ INFO] [1558079427.373060032]: Registered Algorithm [PTP]
[ INFO] [1558079427.373086423]: Using planning interface 'Simple Command Planner'
[ INFO] [1558079427.472360789]: Stereo is NOT SUPPORTED
[ INFO] [1558079427.472421059]: OpenGl version: 3 (GLSL 1.3).
EMCY: 83#0000000000000000
EMCY: 84#0000000000000000
EMCY: 85#0000000000000000
EMCY: 86#0000000000000000
EMCY: 87#0000000000000000
EMCY: 88#0000000000000000
[robot_init-6] process has finished cleanly
log file: /home/hsl/.ros/log/6e64d6b6-7878-11e9-a4b9-509a4c59ef48/robot_init-6*.log
[ INFO] [1558079428.585387643]: waitForService: Service [/prbt/manipulator_joint_trajectory_controller/hold] is now available.
[ INFO] [1558079428.770896167]: Added FollowJointTrajectory controller for prbt/manipulator_joint_trajectory_controller/
[ INFO] [1558079428.771022597]: Returned 1 controllers in list
[ INFO] [1558079428.773113245]: Recovering XXX
[ INFO] [1558079428.784561044]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
Loading 'pilz_trajectory_generation/MoveGroupSequenceAction'...
[ INFO] [1558079428.865252910]: Reading limits from namespace /robot_description_planning
Loading 'pilz_trajectory_generation/MoveGroupSequenceService'...
[ INFO] [1558079428.891871936]: Reading limits from namespace /robot_description_planning
[ INFO] [1558079428.918866748]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
*     - SequenceAction
*     - SequenceService
********************************************************

[ INFO] [1558079428.918963271]: MoveGroup context using planning plugin pilz::CommandPlanner
[ INFO] [1558079428.918996328]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1558079430.809120497]: Loading robot model 'prbt'...
[ WARN] [1558079430.809166964]: Skipping virtual joint 'FixedBase' because its child frame 'prbt_base_link' does not match the URDF frame 'world'
[ INFO] [1558079430.809182588]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1558079430.872455906]: Starting scene monitor
[ INFO] [1558079430.875540761]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1558079431.185451612]: Constructing new MoveGroup connection for group 'manipulator' in namespace ''
[ INFO] [1558079432.403291396]: Ready to take commands for planning group manipulator.
[ INFO] [1558079432.403363002]: Looking around: no
[ INFO] [1558079432.403388401]: Replanning: no
[ WARN] [1558079432.412028183]: Interactive marker 'EE:goal_prbt_flange' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.
[ INFO] [1558079444.591618403]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1558079444.591838345]: Planning attempt 1 of at most 1
[ INFO] [1558079444.592110054]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1558079444.592362998]: Generating PTP trajectory...
[ INFO] [1558079445.611700625]: Performing STOP due to STO signal
[ WARN] [1558079445.613169774]: Controller handle prbt/manipulator_joint_trajectory_controller/ reports status PREEMPTED
[ INFO] [1558079445.613211361]: Completed trajectory execution with status PREEMPTED ...
[ INFO] [1558079445.814444598]: Halting down XXX
[ INFO] [1558079445.824676851]: ABORTED: Solution found but controller failed during execution
EMCY: 87#0010812903000000
EMCY: 88#0010812903000000
EMCY: 84#0010812903000000
EMCY: 85#0010812903000000
EMCY: 86#0010812903000000
EMCY: 83#0010812903000000
[ INFO] [1558079452.746972692]: Recovering XXX
ER: 129
[ INFO] [1558079456.071845209]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1558079456.071918379]: Planning attempt 1 of at most 1
[ INFO] [1558079456.072009962]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1558079456.072079169]: Generating PTP trajectory...
[ WARN] [1558079456.885477023]: Controller prbt/manipulator_joint_trajectory_controller/ failed with error code PATH_TOLERANCE_VIOLATED
[ WARN] [1558079456.885571385]: Controller handle prbt/manipulator_joint_trajectory_controller/ reports status ABORTED
[ INFO] [1558079456.885591422]: Completed trajectory execution with status ABORTED ...
[ INFO] [1558079457.088095546]: ABORTED: Solution found but controller failed during execution
[ INFO] [1558079462.595499927]: Performing STOP due to STO signal
[ INFO] [1558079462.798405261]: Halting down XXX
hslusarek commented 5 years ago

Bug also appears with melodic-devel (17.05.2019)

hslusarek commented 5 years ago

The following workaround seems to fix the bug: see here (Caution: Do not use the workaround!)

hslusarek commented 5 years ago

Maybe duplicate of #93?

It seems that way yes.

hslusarek commented 5 years ago

Close because we assume that this bug is a duplicate #93.