Error when setting up Gazebo with MoveIt! and Stretch

Ask
1

Continuing the discussion from Error when setting up Stretch simulation on Gazebo:

Now that I have the initial demo running with the teleop_twist, I was trying to run the MoveIt! demo per the ReadMe. Everything works very well on the MoveIt! interface, but when I try to execute a plan, I receive the following error outputs from $ roslaunch stretch_moveit_config demo_gazebo.launch:

[ INFO] [1624391236.290471975, 16.464000000]: Didn’t receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1624391236.290540103, 16.464000000]: Failed to validate trajectory: couldn’t receive full current joint state within 1s
[ INFO] [1624391236.312759822, 16.469000000]: ABORTED: Solution found but controller failed during execution

Since I don’t have a joystick, I made a simple launch file for the teleop_twist_keyboard, but I didn’t see anything that I thought would affect the MoveIt! operation.

I am just learning Gazebo and MoveIt! so I really appreciate the assistance with the setup!

There is much more in the terminal output prior to those errors, but none of it seemed relevant. Just in case, it's here. $ roslaunch stretch_moveit_config demo_gazebo.launch ... logging to /home/hcilab/.ros/log/88c7b84e-d392-11eb-978b-d0577b4196c8/roslaunch-hcilab-HP-Pavilion-24585.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://127.0.0.1:44501/

SUMMARY

PARAMETERS

  • /move_group/allow_trajectory_execution: True
  • /move_group/capabilities:
  • /move_group/controller_list: [{‘default’: True…
  • /move_group/disable_capabilities:
  • /move_group/jiggle_fraction: 0.05
  • /move_group/joint_state_controller/publish_rate: 50
  • /move_group/joint_state_controller/type: joint_state_contr…
  • /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/planner_configs/AnytimePathShortening/hybridize: True
  • /move_group/planner_configs/AnytimePathShortening/max_hybrid_paths: 24
  • /move_group/planner_configs/AnytimePathShortening/num_planners: 4
  • /move_group/planner_configs/AnytimePathShortening/planners:
  • /move_group/planner_configs/AnytimePathShortening/shortcut: True
  • /move_group/planner_configs/AnytimePathShortening/type: geometric::Anytim…
  • /move_group/planner_configs/BFMT/balanced: 0
  • /move_group/planner_configs/BFMT/cache_cc: 1
  • /move_group/planner_configs/BFMT/extended_fmt: 1
  • /move_group/planner_configs/BFMT/heuristics: 1
  • /move_group/planner_configs/BFMT/nearest_k: 1
  • /move_group/planner_configs/BFMT/num_samples: 1000
  • /move_group/planner_configs/BFMT/optimality: 1
  • /move_group/planner_configs/BFMT/radius_multiplier: 1.0
  • /move_group/planner_configs/BFMT/type: geometric::BFMT
  • /move_group/planner_configs/BKPIECE/border_fraction: 0.9
  • /move_group/planner_configs/BKPIECE/failed_expansion_score_factor: 0.5
  • /move_group/planner_configs/BKPIECE/min_valid_path_fraction: 0.5
  • /move_group/planner_configs/BKPIECE/range: 0.0
  • /move_group/planner_configs/BKPIECE/type: geometric::BKPIECE
  • /move_group/planner_configs/BiEST/range: 0.0
  • /move_group/planner_configs/BiEST/type: geometric::BiEST
  • /move_group/planner_configs/BiTRRT/cost_threshold: 1e300
  • /move_group/planner_configs/BiTRRT/frountier_node_ratio: 0.1
  • /move_group/planner_configs/BiTRRT/frountier_threshold: 0.0
  • /move_group/planner_configs/BiTRRT/init_temperature: 100
  • /move_group/planner_configs/BiTRRT/range: 0.0
  • /move_group/planner_configs/BiTRRT/temp_change_factor: 0.1
  • /move_group/planner_configs/BiTRRT/type: geometric::BiTRRT
  • /move_group/planner_configs/EST/goal_bias: 0.05
  • /move_group/planner_configs/EST/range: 0.0
  • /move_group/planner_configs/EST/type: geometric::EST
  • /move_group/planner_configs/FMT/cache_cc: 1
  • /move_group/planner_configs/FMT/extended_fmt: 1
  • /move_group/planner_configs/FMT/heuristics: 0
  • /move_group/planner_configs/FMT/nearest_k: 1
  • /move_group/planner_configs/FMT/num_samples: 1000
  • /move_group/planner_configs/FMT/radius_multiplier: 1.1
  • /move_group/planner_configs/FMT/type: geometric::FMT
  • /move_group/planner_configs/KPIECE/border_fraction: 0.9
  • /move_group/planner_configs/KPIECE/failed_expansion_score_factor: 0.5
  • /move_group/planner_configs/KPIECE/goal_bias: 0.05
  • /move_group/planner_configs/KPIECE/min_valid_path_fraction: 0.5
  • /move_group/planner_configs/KPIECE/range: 0.0
  • /move_group/planner_configs/KPIECE/type: geometric::KPIECE
  • /move_group/planner_configs/LBKPIECE/border_fraction: 0.9
  • /move_group/planner_configs/LBKPIECE/min_valid_path_fraction: 0.5
  • /move_group/planner_configs/LBKPIECE/range: 0.0
  • /move_group/planner_configs/LBKPIECE/type: geometric::LBKPIECE
  • /move_group/planner_configs/LBTRRT/epsilon: 0.4
  • /move_group/planner_configs/LBTRRT/goal_bias: 0.05
  • /move_group/planner_configs/LBTRRT/range: 0.0
  • /move_group/planner_configs/LBTRRT/type: geometric::LBTRRT
  • /move_group/planner_configs/LazyPRM/range: 0.0
  • /move_group/planner_configs/LazyPRM/type: geometric::LazyPRM
  • /move_group/planner_configs/LazyPRMstar/type: geometric::LazyPR…
  • /move_group/planner_configs/PDST/type: geometric::PDST
  • /move_group/planner_configs/PRM/max_nearest_neighbors: 10
  • /move_group/planner_configs/PRM/type: geometric::PRM
  • /move_group/planner_configs/PRMstar/type: geometric::PRMstar
  • /move_group/planner_configs/ProjEST/goal_bias: 0.05
  • /move_group/planner_configs/ProjEST/range: 0.0
  • /move_group/planner_configs/ProjEST/type: geometric::ProjEST
  • /move_group/planner_configs/RRT/goal_bias: 0.05
  • /move_group/planner_configs/RRT/range: 0.0
  • /move_group/planner_configs/RRT/type: geometric::RRT
  • /move_group/planner_configs/RRTConnect/range: 0.0
  • /move_group/planner_configs/RRTConnect/type: geometric::RRTCon…
  • /move_group/planner_configs/RRTstar/delay_collision_checking: 1
  • /move_group/planner_configs/RRTstar/goal_bias: 0.05
  • /move_group/planner_configs/RRTstar/range: 0.0
  • /move_group/planner_configs/RRTstar/type: geometric::RRTstar
  • /move_group/planner_configs/SBL/range: 0.0
  • /move_group/planner_configs/SBL/type: geometric::SBL
  • /move_group/planner_configs/SPARS/dense_delta_fraction: 0.001
  • /move_group/planner_configs/SPARS/max_failures: 1000
  • /move_group/planner_configs/SPARS/sparse_delta_fraction: 0.25
  • /move_group/planner_configs/SPARS/stretch_factor: 3.0
  • /move_group/planner_configs/SPARS/type: geometric::SPARS
  • /move_group/planner_configs/SPARStwo/dense_delta_fraction: 0.001
  • /move_group/planner_configs/SPARStwo/max_failures: 5000
  • /move_group/planner_configs/SPARStwo/sparse_delta_fraction: 0.25
  • /move_group/planner_configs/SPARStwo/stretch_factor: 3.0
  • /move_group/planner_configs/SPARStwo/type: geometric::SPARStwo
  • /move_group/planner_configs/STRIDE/degree: 16
  • /move_group/planner_configs/STRIDE/estimated_dimension: 0.0
  • /move_group/planner_configs/STRIDE/goal_bias: 0.05
  • /move_group/planner_configs/STRIDE/max_degree: 18
  • /move_group/planner_configs/STRIDE/max_pts_per_leaf: 6
  • /move_group/planner_configs/STRIDE/min_degree: 12
  • /move_group/planner_configs/STRIDE/min_valid_path_fraction: 0.2
  • /move_group/planner_configs/STRIDE/range: 0.0
  • /move_group/planner_configs/STRIDE/type: geometric::STRIDE
  • /move_group/planner_configs/STRIDE/use_projected_distance: 0
  • /move_group/planner_configs/TRRT/frountierNodeRatio: 0.1
  • /move_group/planner_configs/TRRT/frountier_threshold: 0.0
  • /move_group/planner_configs/TRRT/goal_bias: 0.05
  • /move_group/planner_configs/TRRT/init_temperature: 10e-6
  • /move_group/planner_configs/TRRT/k_constant: 0.0
  • /move_group/planner_configs/TRRT/max_states_failed: 10
  • /move_group/planner_configs/TRRT/min_temperature: 10e-10
  • /move_group/planner_configs/TRRT/range: 0.0
  • /move_group/planner_configs/TRRT/temp_change_factor: 2.0
  • /move_group/planner_configs/TRRT/type: geometric::TRRT
  • /move_group/planning_plugin: ompl_interface/OM…
  • /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: default_planner_r…
  • /move_group/sensors: [{'point_subsampl…
  • /move_group/start_state_max_bounds_error: 0.1
  • /move_group/stretch_arm/default_planner_config: RRTConnect
  • /move_group/stretch_arm/planner_configs: ['AnytimePathShor…
  • /move_group/stretch_gripper/default_planner_config: RRTConnect
  • /move_group/stretch_gripper/planner_configs: ['AnytimePathShor…
  • /move_group/stretch_head/default_planner_config: RRTConnect
  • /move_group/stretch_head/planner_configs: ['AnytimePathShor…
  • /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
  • /robot_description: <?xml version="1…
  • /robot_description_kinematics/stretch_arm/kinematics_solver: kdl_kinematics_pl…
  • /robot_description_kinematics/stretch_arm/kinematics_solver_search_resolution: 0.005
  • /robot_description_kinematics/stretch_arm/kinematics_solver_timeout: 0.005
  • /robot_description_kinematics/stretch_head/kinematics_solver: kdl_kinematics_pl…
  • /robot_description_kinematics/stretch_head/kinematics_solver_search_resolution: 0.005
  • /robot_description_kinematics/stretch_head/kinematics_solver_timeout: 0.1
  • /robot_description_planning/default_acceleration_scaling_factor: 0.1
  • /robot_description_planning/default_velocity_scaling_factor: 0.1
  • /robot_description_planning/joint_limits/joint_arm_l0/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l0/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l0/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l0/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_arm_l1/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l1/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l1/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l1/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_arm_l2/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l2/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l2/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l2/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_arm_l3/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l3/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l3/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l3/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_lift/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_lift/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_lift/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_lift/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_wrist_yaw/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_wrist_yaw/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_wrist_yaw/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_wrist_yaw/max_velocity: 1
  • /robot_description_semantic: <?xml version="1…
  • /rosdistro: melodic
  • /rosversion: 1.14.10

NODES
/
move_group (moveit_ros_move_group/move_group)
rviz_hcilab_HP_Pavilion_24585_8593209250659358117 (rviz/rviz)
virtual_joint_broadcaster_0 (tf2_ros/static_transform_publisher)

ROS_MASTER_URI=http://localhost:11311

process[virtual_joint_broadcaster_0-1]: started with pid [24644]
process[move_group-2]: started with pid [24645]
process[rviz_hcilab_HP_Pavilion_24585_8593209250659358117-3]: started with pid [24650]
[ INFO] [1624391209.429076265]: Loading robot model ‘stretch_description’…
[ INFO] [1624391209.430194019]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1624391209.522116464]: rviz version 1.13.17
[ INFO] [1624391209.522199886]: compiled against Qt version 5.9.5
[ INFO] [1624391209.522223436]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1624391209.528010789]: Forcing OpenGl version 0.
[ WARN] [1624391209.726635487, 6.675000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391209.733804592, 6.678000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1624391210.563529605, 6.949000000]: Publishing maintained planning scene on ‘monitored_planning_scene’
[ INFO] [1624391210.567065540, 6.950000000]: MoveGroup debug mode is ON
Starting planning scene monitors…
[ INFO] [1624391210.567109979, 6.950000000]: Starting planning scene monitor
[ INFO] [1624391210.569398280, 6.951000000]: Listening to ‘/planning_scene’
[ INFO] [1624391210.569430219, 6.951000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1624391210.572020157, 6.952000000]: Listening to ‘/collision_object’
[ INFO] [1624391210.573589682, 6.952000000]: Listening to ‘/planning_scene_world’ for planning scene world geometry
[ INFO] [1624391210.665396361, 6.982000000]: Stereo is NOT SUPPORTED
[ INFO] [1624391210.665495789, 6.982000000]: OpenGL device: GeForce GTX 960M/PCIe/SSE2
[ INFO] [1624391210.665563776, 6.982000000]: OpenGl version: 4.6 (GLSL 4.6).
[ WARN] [1624391210.684220753, 6.988000000]: Convex hull creation failed
[ WARN] [1624391210.684335455, 6.988000000]: Convex hull creation failed
[ INFO] [1624391212.682383131, 7.730000000]: Listening to ‘/head_mount_kinect/depth_registered/points’ using message filter with target frame 'base_link ’
[ INFO] [1624391212.686262401, 7.731000000]: Listening to ‘/attached_collision_object’ for attached collision objects
Planning scene monitors started.
[ INFO] [1624391212.713561005, 7.741000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1624391212.755362156, 7.756000000]: Using planning interface ‘OMPL’
[ INFO] [1624391212.759363849, 7.758000000]: Param ‘default_workspace_bounds’ was not set. Using default value: 10
[ INFO] [1624391212.759828534, 7.758000000]: Param ‘start_state_max_bounds_error’ was set to 0.1
[ INFO] [1624391212.760221692, 7.758000000]: Param ‘start_state_max_dt’ was not set. Using default value: 0.5
[ INFO] [1624391212.760740347, 7.759000000]: Param ‘start_state_max_dt’ was not set. Using default value: 0.5
[ INFO] [1624391212.761124518, 7.759000000]: Param ‘jiggle_fraction’ was set to 0.05
[ INFO] [1624391212.761504321, 7.759000000]: Param ‘max_sampling_attempts’ was not set. Using default value: 100
[ INFO] [1624391212.761556067, 7.759000000]: Using planning request adapter ‘Add Time Parameterization’
[ INFO] [1624391212.761578265, 7.759000000]: Using planning request adapter ‘Fix Workspace Bounds’
[ INFO] [1624391212.761600973, 7.759000000]: Using planning request adapter ‘Fix Start State Bounds’
[ INFO] [1624391212.761623944, 7.759000000]: Using planning request adapter ‘Fix Start State In Collision’
[ INFO] [1624391212.761641439, 7.759000000]: Using planning request adapter ‘Fix Start State Path Constraints’
[ INFO] [1624391212.984467244, 7.846000000]: Added FollowJointTrajectory controller for stretch_arm_controller
[ INFO] [1624391213.273770230, 7.946000000]: Added FollowJointTrajectory controller for stretch_head_controller
[ INFO] [1624391213.488519744, 8.019000000]: Added FollowJointTrajectory controller for stretch_gripper_controller
[ INFO] [1624391213.488627166, 8.019000000]: Returned 3 controllers in list
[ INFO] [1624391213.501528525, 8.022000000]: 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’…
[ INFO] [1624391213.562829052, 8.043000000]:

  • MoveGroup using:
  • - ApplyPlanningSceneService

  • - ClearOctomapService

  • - CartesianPathService

  • - ExecuteTrajectoryAction

  • - GetPlanningSceneService

  • - KinematicsService

  • - MoveAction

  • - PickPlaceAction

  • - MotionPlanService

  • - QueryPlannersService

  • - StateValidationService

[ INFO] [1624391213.562906681, 8.043000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1624391213.562937034, 8.043000000]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1624391214.049193554, 8.220000000]: Loading robot model ‘stretch_description’…
[ INFO] [1624391214.049257922, 8.220000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1624391214.326861418, 8.317000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391214.335422661, 8.320000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1624391215.171264846, 8.621000000]: Starting planning scene monitor
[ INFO] [1624391215.173783693, 8.622000000]: Listening to ‘/move_group/monitored_planning_scene’
[ WARN] [1624391216.481111437, 9.065000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391216.481310627, 9.065000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391216.487478361, 9.067000000]: Unable to transform object from frame ‘map’ to planning frame ‘base_link’ (Could not find a connection between ‘base_link’ and ‘map’ because they are not part of the same tree.Tf has two or more unconnected trees.)
[ INFO] [1624391216.488590112, 9.067000000]: Constructing new MoveGroup connection for group ‘stretch_arm’ in namespace ‘’
[ INFO] [1624391217.584871065, 9.475000000]: Ready to take commands for planning group stretch_arm.
[ INFO] [1624391217.585017244, 9.476000000]: Looking around: no
[ INFO] [1624391217.585053278, 9.476000000]: Replanning: no
[ INFO] [1624391235.042137248, 16.000000000]: Didn’t receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1624391235.042203297, 16.000000000]: Failed to fetch current robot state.
[ WARN] [1624391235.042285753, 16.000000000]: Unable to transform object from frame ‘map’ to planning frame ‘base_link’ (Could not find a connection between ‘base_link’ and ‘map’ because they are not part of the same tree.Tf has two or more unconnected trees.)
[ INFO] [1624391235.042391243, 16.000000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1624391235.042574288, 16.000000000]: Planning attempt 1 of at most 1
[ INFO] [1624391235.044634186, 16.000000000]: Planner configuration ‘stretch_arm[RRTConnect]’ will use planner ‘geometric::RRTConnect’. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1624391235.045747809, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.045799494, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.045852315, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.045958093, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.072180338, 16.009000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.073048387, 16.009000000]: stretch_arm[RRTConnect]: Created 7 states (2 start + 5 goal)
[ INFO] [1624391235.074481000, 16.010000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.078630898, 16.012000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.081267629, 16.013000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.036013 seconds
[ INFO] [1624391235.081541725, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.081720184, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.083426927, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.083548021, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.095424976, 16.017000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.097279729, 16.017000000]: stretch_arm[RRTConnect]: Created 8 states (2 start + 6 goal)
[ INFO] [1624391235.099494653, 16.017000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.101233910, 16.018000000]: stretch_arm[RRTConnect]: Created 7 states (2 start + 5 goal)
[ INFO] [1624391235.111613714, 16.021000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.030130 seconds
[ INFO] [1624391235.111791181, 16.021000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.111856208, 16.021000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.122429290, 16.024000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.125721590, 16.025000000]: stretch_arm[RRTConnect]: Created 7 states (2 start + 5 goal)
[ INFO] [1624391235.134678419, 16.026000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.022911 seconds
[ INFO] [1624391235.275166648, 16.079000000]: SimpleSetup: Path simplification took 0.140280 seconds and changed from 5 to 2 states
[ INFO] [1624391235.290303685, 16.085000000]: Disabling trajectory recording
[ INFO] [1624391236.290471975, 16.464000000]: Didn’t receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1624391236.290540103, 16.464000000]: Failed to validate trajectory: couldn’t receive full current joint state within 1s
[ INFO] [1624391236.312759822, 16.469000000]: ABORTED: Solution found but controller failed during execution

2

Drive / Joystick should be irrelevant to moveit you are correct on that.

I can’t reproduce this, but it seems to me either joint_states aren’t published correctly from Gazebo or there is something wrong with your clock. I have a few questions to better understand your problem:

  • Can you visualize the robot in Rviz?
  • What is your real time factor in gazebo (located in Left Bottom)?
  • Are you running ROS in multiple machines without clock sync?
  • Are you setting use_sim_time parameter to false in the launch file?

In a relevant note, if you are new to MoveIt! I recommend giving the official tutorials a look as well as Stretch

3

One thing may work is you can try removing <arg name="use_sim_time" value="$(arg use_sim_time)"/> from gazebo.launch file (should be on line 15).

4

Thanks for the help, and the tutorials–I’ll be sure to check them out! To answer your questions:

  • The movit rviz window visualizes fine, including the arm movements when I try to execute the plan. However, the rviz window produce by gazebo.launch with rviz:=true produces a broken robot:

    rviz
    rviz917×775 35.3 KB

  • The real time factor in Gazebo seems to fluctuate between 0.35-0.38 as the simulation is running.

  • I’m just using ROS on one machine

  • I didn’t set use_sim_time in the launch file, so it’s just using the default of “true”

I tried removing <arg name="use_sim_time" value="$(arg use_sim_time)"/> from gazebo.launch file, but it did not affect the simulation that I could tell.

5

Update: I solved the issue–joint_state_controller was not properly installed (I had thought it was, but I guess not).

Got that fixed, and now MoveIt! works, and the rviz window from gazebo.launch ino longer has a broken robot.

Thanks!

2 Likes
6

Thanks @lstegner, joint_state_controller is now an explicit dependency of stretch_gazebo. It’ll install automatically when using rosdep. I’ve also added a keyboard_teleop script!

1 Like
7

Hi Binit!

I have been exploring the stretch_gazebo and was successful using the teleop_twist.launch file once you remap the cmd_vel topic. I, however, could not do the same with the keyboard_teleop one. The launch file first launches stretch_driver and it requires stretch_body.

Is there a way to include lift, wrist and head teleoperation with gazebo in a similar way to teleop_twist?

8

Hey @csemecu, Thanks for pointing out the need for complete teleop control of Stretch within the Gazebo simulation. Yes, the regular keyboard_teleop node wouldn’t support the Gazebo simulation because it is scripted to be used with the stretch_driver node and the controller configuration is different for the Stretch’s gazebo implementation.

I have modified the regular keyboard teleop node to add the keyboard_teleop_gazebo node which should now support the controller configuration of Stretch’s Gazebo configuration and would enable you to teleoperate Stretch fully (lift, head, arm, gripper, base) within Gazebo in the same fashion.

You can try out this full keyboard teleop control for the Stretch gazebo with the following steps:

Switch your stretch_ros repository to development branch feature/keyboard_teleop_gazebo

cd ~/catkin_ws/src/stretch_ros
git pull
git checkout feature/keyboard_teleop_gazebo

#Terminal 1
roslaunch stretch_gazebo gazebo.launch

#Terminal 2
rosrun stretch_gazebo keyboard_teleop_gazebo

Let us know if you find any issues.
Thanks!

1 Like
9

Thank you!!

I will be trying this out in the next few days and will let you know how it goes posting either a reply here or in the Share part.

1 Like
10

I’m sorry I never got around to replying here in the summer. As you may notice I created a new user because I also lost access to my UW account.
Anyways! I tried the keyboard teleop with the gazebo version and it does move all the joints (except the base, but I have seen the “navigation mode” in other places so that would probably address it). A couple of things I noticed, some of the keys pressed are not detected (almost always the only key that is detected triggers a refresh on the terminal to display the menu again). Do you think you will be incorporating this to the master branch?

1 Like
11

I suspect the undetected key presses issue happens because this FollowJointTrajectory goal takes 0.2 seconds to complete, whereas the origin keyboard teleop could issue goals in close succession because stretch_driver supports preemption. Given this constraint, I’d recommend adding an waitForResult blocking call after this client sends the goal.

I think it would be great to get this node merged into the noetic branch. What do you think @Mohamed_Fazil? You could cherry-pick the two commits onto a new branch based off “dev/noetic”, and open a PR. I’d be happy to be a reviewer.

1 Like
12

Hey @bshah, Sure even I think it is a good idea to have the Keyboard teleop node for gazebo standard with Noetic. I can start working on it.

13

Hi @umlmecu and @Mohamed_Fazil,

Teleop for Gazebo Stretch is merged! Try it out by refreshing your ROS workspace and following these instructions.

1 Like