Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[gazebo] Add headmount cameras. #330

Open
wants to merge 5 commits into
base: indigo-devel
Choose a base branch
from

Conversation

130s
Copy link
Contributor

@130s 130s commented May 18, 2017

nxo_gz_ueye_cameras_rviz

NEXTAGE Open by default comes with IDS Ueye cameras, emulator of which is not available on Gazebo simulator. Instead the standard camera implementation on Gazebo is used.

nxo_gz_ueye_camerasnxo_gz_ueye_cameras_wireframe
Camera link in this PR uses a cylindrical object. As you may see in the wireframe above, short cylinders are "embedded" in the eye cameras.

nxo_gz_headmountcams_handsinsight_rviz_rqt

NOTE: The poses of headmount cameras and the camera's physical parameters are NOT at all precisely adjusted to the 3D model nor the actual hardware spec. These configs are meant for a rapid prototyping only. If you need precise configuration, please go ahead modify, then (if possible) send a patch to https://github.com/tork-a/rtmros_nextage/pulls so that other developers and users can use (and possible improve).

Ignoring the precision, adding hand cameras should be easy using the same xacro in this PR.

FYI @y-yosuke
I had no idea if there's even a way to adjust the emulated headmount camera component to the actual setting (available in VRML, Collada, and of course URDF). If anyone can share how to do so that'll be appreciated.

@130s
Copy link
Contributor Author

130s commented May 20, 2017

A testcase using botharms from nextage_moveit_config kept failing ONLY for nextage_gazebo. The same testcase in nextage_moveit_config pkg does fine.

Log example:

:
[ INFO] [1495234653.684574153, 11.850000000]: Found a contact between 'HEAD_JOINT1_Link' (type 'Robot link') and 'R_HEADMOUNT_CAMERA_Link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1495234653.684647169, 11.850000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1495234653.684715390, 11.850000000]: Start state appears to be in collision with respect to group botharms
�[33m[ WARN] [1495234653.898955750, 12.043000000]: Unable to find a valid state nearby the start state (using jiggle fraction of 0.050000 and 100 sampling attempts). Passing the original planning request to the planner.
[ INFO] [1495234653.900194387, 12.044000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1495234653.900291307, 12.044000000]: Planner configuration 'botharms[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
�[33m[ WARN] [1495234653.900680637, 12.044000000]: botharms[RRTConnectkConfigDefault]: Skipping invalid start state (invalid state)
�[31m[ERROR] [1495234653.900789923, 12.044000000]: botharms[RRTConnectkConfigDefault]: Motion planning start tree could not be initialized!

I suspected that moveit_config might have to be updated once URDF is changed (reference), but then locally the same testcase runs fine (as following, slightly modified to remove unittest-specific code) without updating moveit_config.

In [1]: from geometry_msgs.msg import Pose
In [2]: from moveit_commander import MoveGroupCommander, MoveItCommanderException, RobotCommander
In [3]: from moveit_msgs.msg import RobotTrajectory
In [4]: import rospy
In [5]: import tf
In [7]: from tf.transformations import quaternion_from_euler
In [24]: import math
In [25]:         target_pose_r = Pose()
In [26]:         target_pose_l = Pose()
In [27]:         q = quaternion_from_euler(0, -math.pi/2,0)
In [28]:         target_pose_r.position.x = 0.3
In [29]:         target_pose_r.position.y = 0.1
In [30]:         target_pose_r.position.z = 0.0
In [31]:         target_pose_r.orientation.x = q[0]
In [32]:         target_pose_r.orientation.y = q[1]
In [33]:         target_pose_r.orientation.z = q[2]
In [34]:         target_pose_r.orientation.w = q[3]
In [35]:         target_pose_l.position.x = 0.3
In [36]:         target_pose_l.position.y =-0.1
In [37]:         target_pose_l.position.z = 0.3
In [38]:         target_pose_l.orientation.x = q[0]
In [39]:         target_pose_l.orientation.y = q[1]
In [40]:         target_pose_l.orientation.z = q[2]
In [41]:         target_pose_l.orientation.w = q[3]
In [42]: mg = MoveGroupCommander('botharms')
[ INFO] [1495233288.265543989]: Loading robot model 'NextageOpen'...
[ INFO] [1495233289.418649101, 290.951000000]: TrajectoryExecution will use old service capability.
[ INFO] [1495233289.418741652, 290.951000000]: Ready to take MoveGroup commands for group botharms.
In [45]: botharms=mg
In [46]: botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
In [46]: botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
In [47]:         botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
In [48]:         botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
In [49]: botharms.go()
Out[49]: True
In [50]:         target_pose_r.position.x = 0.3
In [51]:         target_pose_r.position.y =-0.2
In [52]:         target_pose_r.position.z = 0.0
In [53]:         target_pose_l.position.x = 0.3
In [54]:         target_pose_l.position.y = 0.2
In [55]:         target_pose_l.position.z = 0.0
In [56]:         botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
In [57]:         botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
In [58]: botharms.go()O
ut[58]: True

@130s 130s force-pushed the impr/gazebo_ueye branch from 6d256b4 to 48d9a6d Compare May 27, 2017 02:03
@130s
Copy link
Contributor Author

130s commented May 27, 2017

Locally running then I don't see the warning message on the CI in #330 (comment) are printed. And the robot on Gazebo (and on RViz) seem to be moving synchronized using botharms move group. One thing is that the controller execution is aborted. Anyway I'm not sure what makes the test for botharms move group fail only on CI. Hmm...

$ roscd nextage_moveit_config
$ git log              
commit 48d9a6da7f6b4d1f41073a56769e1b9b1a5b1809
Author: Isaac I.Y. Saito <[email protected]>
Date:   Thu May 18 15:20:19 2017 -0700

    [Gazebo] Unwind the frequency test.
    
    hz requirement for Gazebo sensor plugin on Travis CI needs to be generous. There's a known issue about
:
$ roslaunch nextage_moveit_config moveit_planning_execution.launch
... logging to /home/robnoodle/.ros/log/0dfca28a-4293-11e7-9529-80fa5b08d1d8/roslaunch-tork-kudu1-467.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://tork-kudu1:45649/

SUMMARY
========

PARAMETERS
 * /move_group/allow_trajectory_execution: True
 * /move_group/botharms/default_planner_config: RRTConnectkConfig...
 * /move_group/botharms/longest_valid_segment_fraction: 0.05
 * /move_group/botharms/planner_configs: ['SBLkConfigDefau...
 * /move_group/botharms/projection_evaluator: joints(LARM_JOINT...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/controller_manager_name: pr2_controller_ma...
 * /move_group/controller_manager_ns: pr2_controller_ma...
 * /move_group/head/default_planner_config: RRTConnectkConfig...
 * /move_group/head/longest_valid_segment_fraction: 0.05
 * /move_group/head/planner_configs: ['SBLkConfigDefau...
 * /move_group/head/projection_evaluator: joints(HEAD_JOINT...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/left_arm/default_planner_config: RRTConnectkConfig...
 * /move_group/left_arm/kinematics_solver: kdl_kinematics_pl...
 * /move_group/left_arm/kinematics_solver_attempts: 3
 * /move_group/left_arm/kinematics_solver_search_resolution: 0.005
 * /move_group/left_arm/kinematics_solver_timeout: 0.005
 * /move_group/left_arm/longest_valid_segment_fraction: 0.05
 * /move_group/left_arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/left_arm/projection_evaluator: joints(LARM_JOINT...
 * /move_group/left_arm_torso/kinematics_solver: kdl_kinematics_pl...
 * /move_group/left_arm_torso/kinematics_solver_attempts: 3
 * /move_group/left_arm_torso/kinematics_solver_search_resolution: 0.005
 * /move_group/left_arm_torso/kinematics_solver_timeout: 0.005
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: pr2_moveit_contro...
 * /move_group/moveit_manage_controllers: True
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/TRRTkConfigDefault/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/right_arm/default_planner_config: RRTConnectkConfig...
 * /move_group/right_arm/kinematics_solver: kdl_kinematics_pl...
 * /move_group/right_arm/kinematics_solver_attempts: 3
 * /move_group/right_arm/kinematics_solver_search_resolution: 0.005
 * /move_group/right_arm/kinematics_solver_timeout: 0.005
 * /move_group/right_arm/longest_valid_segment_fraction: 0.05
 * /move_group/right_arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/right_arm/projection_evaluator: joints(RARM_JOINT...
 * /move_group/right_arm_torso/kinematics_solver: kdl_kinematics_pl...
 * /move_group/right_arm_torso/kinematics_solver_attempts: 3
 * /move_group/right_arm_torso/kinematics_solver_search_resolution: 0.005
 * /move_group/right_arm_torso/kinematics_solver_timeout: 0.005
 * /move_group/start_state_max_bounds_error: 0.1
 * /move_group/torso/default_planner_config: RRTConnectkConfig...
 * /move_group/torso/longest_valid_segment_fraction: 0.05
 * /move_group/torso/planner_configs: ['SBLkConfigDefau...
 * /move_group/torso/projection_evaluator: joints(CHEST_JOINT0)
 * /move_group/upperbody/default_planner_config: RRTConnectkConfig...
 * /move_group/upperbody/longest_valid_segment_fraction: 0.05
 * /move_group/upperbody/planner_configs: ['SBLkConfigDefau...
 * /move_group/upperbody/projection_evaluator: joints(LARM_JOINT...
 * /move_group/use_controller_manager: False
 * /robot_description: <?xml version="1....
 * /robot_description_planning/joint_limits/LARM_JOINT0/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT0/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT0/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT0/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT1/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT1/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT1/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT2/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT2/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT2/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT3/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT3/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT3/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT4/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT4/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT4/max_velocity: 1.0
 * /robot_description_planning/joint_limits/LARM_JOINT5/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/LARM_JOINT5/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/LARM_JOINT5/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT0/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT0/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT0/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT0/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT1/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT1/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT1/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT2/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT2/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT2/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT3/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT3/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT3/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT4/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT4/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT4/max_velocity: 1.0
 * /robot_description_planning/joint_limits/RARM_JOINT5/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/RARM_JOINT5/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/RARM_JOINT5/max_velocity: 1.0
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm/kinematics_solver: kdl_kinematics_pl...
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm/kinematics_solver_attempts: 3
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm/kinematics_solver_search_resolution: 0.005
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm/kinematics_solver_timeout: 0.005
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm_torso/kinematics_solver: kdl_kinematics_pl...
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm_torso/kinematics_solver_attempts: 3
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm_torso/kinematics_solver_search_resolution: 0.005
 * /rviz_tork_kudu1_467_5789727499357910355/left_arm_torso/kinematics_solver_timeout: 0.005
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm/kinematics_solver: kdl_kinematics_pl...
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm/kinematics_solver_attempts: 3
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm/kinematics_solver_search_resolution: 0.005
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm/kinematics_solver_timeout: 0.005
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm_torso/kinematics_solver: kdl_kinematics_pl...
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm_torso/kinematics_solver_attempts: 3
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm_torso/kinematics_solver_search_resolution: 0.005
 * /rviz_tork_kudu1_467_5789727499357910355/right_arm_torso/kinematics_solver_timeout: 0.005

NODES
  /
    move_group (moveit_ros_move_group/move_group)
    rviz_tork_kudu1_467_5789727499357910355 (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[move_group-1]: started with pid [485]
process[rviz_tork_kudu1_467_5789727499357910355-2]: started with pid [486]
[ INFO] [1495858512.825199621]: rviz version 1.11.15
[ INFO] [1495858512.825401888]: compiled against Qt version 4.8.6
[ INFO] [1495858512.825474559]: compiled against OGRE version 1.8.1 (Byatis)
[ INFO] [1495858512.891450560]: Loading robot model 'NextageOpen'...
[ INFO] [1495858513.061341893]: Stereo is NOT SUPPORTED
[ INFO] [1495858513.061556262]: OpenGl version: 3 (GLSL 1.3).
[ INFO] [1495858513.157096801, 7.878000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858513.206940247, 7.927000000]: The root link WAIST 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] [1495858513.216064356, 7.936000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858513.272690355, 7.989000000]: The root link WAIST 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] [1495858513.279910731, 7.996000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858513.337223078, 8.049000000]: The root link WAIST 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] [1495858513.347603815, 8.059000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858513.398934635, 8.111000000]: The root link WAIST 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] [1495858513.494444723, 8.207000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1495858513.502705232, 8.215000000]: MoveGroup debug mode is OFF
Starting context monitors...
[ INFO] [1495858513.502760601, 8.216000000]: Starting scene monitor
[ INFO] [1495858513.508109039, 8.221000000]: Listening to '/planning_scene'
[ INFO] [1495858513.508148725, 8.221000000]: Starting world geometry monitor
[ INFO] [1495858513.518469364, 8.229000000]: Listening to '/collision_object' using message notifier with target frame '/WAIST '
[ INFO] [1495858513.528843787, 8.239000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1495858513.530533922, 8.241000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1495858513.551409544, 8.263000000]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1495858513.617443214, 8.330000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1495858513.694279093, 8.407000000]: Using planning interface 'OMPL'
[ INFO] [1495858513.737539832, 8.450000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1495858513.738447402, 8.451000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1495858513.739240837, 8.452000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1495858513.740167573, 8.453000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1495858513.741046824, 8.454000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1495858513.741800494, 8.455000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1495858513.741877277, 8.455000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1495858513.741920659, 8.455000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1495858513.741940532, 8.455000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1495858513.741957737, 8.455000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1495858513.741995666, 8.455000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1495858513.839557967, 8.550000000]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
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] [1495858514.053527422, 8.760000000]:

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

[ INFO] [1495858514.053757104, 8.760000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1495858514.053826928, 8.760000000]: MoveGroup context initialization complete

All is well! Everyone is happy! You can start planning now!

[ INFO] [1495858516.761152919, 11.458000000]: Loading robot model 'NextageOpen'...
[ INFO] [1495858517.007049399, 11.703000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858517.060841226, 11.758000000]: The root link WAIST 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] [1495858517.076049390, 11.773000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858517.141292754, 11.838000000]: The root link WAIST 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] [1495858517.147839154, 11.845000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858517.197116705, 11.894000000]: The root link WAIST 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] [1495858517.207534509, 11.904000000]: Loading robot model 'NextageOpen'...
[ WARN] [1495858517.256708803, 11.953000000]: The root link WAIST 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] [1495858517.374612871, 12.071000000]: Starting scene monitor
[ INFO] [1495858517.379935382, 12.076000000]: Listening to '/move_group/monitored_planning_scene'
[ WARN] [1495858517.538370334, 12.234000000]: The root link WAIST 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] [1495858517.538565515, 12.234000000]: The root link WAIST 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] [1495858517.538643661, 12.234000000]: The root link WAIST 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] [1495858517.538708491, 12.234000000]: The root link WAIST 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] [1495858517.538769512, 12.235000000]: The root link WAIST 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] [1495858517.541867103, 12.238000000]: Constructing new MoveGroup connection for group 'right_arm' in namespace ''
[ INFO] [1495858518.308625229, 12.999000000]: TrajectoryExecution will use old service capability.
[ INFO] [1495858518.308865451, 13.000000000]: Ready to take MoveGroup commands for group right_arm.
[ INFO] [1495858518.308959226, 13.000000000]: Looking around: no
[ INFO] [1495858518.312104328, 13.002000000]: Replanning: no
[ INFO] [1495858527.903859689, 22.568000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1495858527.904040162, 22.568000000]: Planning attempt 1 of at most 1
[ INFO] [1495858527.908639851, 22.573000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1495858527.909136688, 22.573000000]: Planner configuration 'right_arm[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1495858527.911835886, 22.576000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.911897483, 22.576000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.911958967, 22.576000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.912513668, 22.576000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.923590058, 22.588000000]: right_arm[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858527.923825144, 22.588000000]: right_arm[RRTConnectkConfigDefault]: Created 6 states (2 start + 4 goal)
[ INFO] [1495858527.923947943, 22.588000000]: right_arm[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858527.924040651, 22.588000000]: right_arm[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858527.924549084, 22.589000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.014576 seconds
[ INFO] [1495858527.924729493, 22.589000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.924763501, 22.589000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.924807821, 22.589000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.924847307, 22.589000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.925811712, 22.590000000]: right_arm[RRTConnectkConfigDefault]: Created 5 states (3 start + 2 goal)
[ INFO] [1495858527.926418153, 22.591000000]: right_arm[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858527.926471630, 22.591000000]: right_arm[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858527.926519587, 22.591000000]: right_arm[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858527.926651576, 22.591000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.001992 seconds
[ INFO] [1495858527.926743213, 22.591000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.926775094, 22.591000000]: right_arm[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858527.927558780, 22.592000000]: right_arm[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858527.928319705, 22.593000000]: right_arm[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858527.928459766, 22.593000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.001747 seconds
[ INFO] [1495858527.929011653, 22.593000000]: SimpleSetup: Path simplification took 0.000427 seconds and changed from 3 to 2 states
[ WARN] [1495858529.723253546, 24.383000000]: Controller handle rarm_controller reports status ABORTED
[ INFO] [1495858529.760181142, 24.420000000]: ABORTED: Solution found but controller failed during execution
[ INFO] [1495858541.912950948, 36.549000000]: Constructing new MoveGroup connection for group 'botharms' in namespace ''
[ INFO] [1495858542.069411288, 36.700000000]: TrajectoryExecution will use old service capability.
[ INFO] [1495858542.069471977, 36.700000000]: Ready to take MoveGroup commands for group botharms.
[ INFO] [1495858542.069820629, 36.700000000]: Looking around: no
[ INFO] [1495858542.069947698, 36.701000000]: Replanning: no
[ INFO] [1495858549.183843842, 43.751000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1495858549.183945632, 43.751000000]: Planning attempt 1 of at most 1
[ INFO] [1495858549.184949980, 43.752000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1495858549.184999707, 43.752000000]: Planner configuration 'botharms[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1495858549.185554224, 43.753000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.185622836, 43.753000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.185664434, 43.753000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.185815774, 43.753000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.197610904, 43.765000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858549.197947213, 43.765000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858549.198835669, 43.766000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858549.199691208, 43.767000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858549.199808681, 43.767000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.014690 seconds
[ INFO] [1495858549.200049518, 43.767000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.200127217, 43.767000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.200177297, 43.767000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.201306735, 43.768000000]: botharms[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858549.201654651, 43.768000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (3 start + 2 goal)
[ INFO] [1495858549.201732189, 43.768000000]: botharms[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858549.201775964, 43.768000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.203426161, 43.771000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (3 start + 2 goal)
[ INFO] [1495858549.203592440, 43.771000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.003629 seconds
[ INFO] [1495858549.203725555, 43.771000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.203762730, 43.771000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858549.205161972, 43.772000000]: botharms[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [1495858549.206257467, 43.773000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (3 start + 2 goal)
[ INFO] [1495858549.206422417, 43.774000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.002747 seconds
[ INFO] [1495858549.207277495, 43.774000000]: SimpleSetup: Path simplification took 0.000788 seconds and changed from 3 to 2 states
[ WARN] [1495858551.712722941, 46.274000000]: Controller handle larm_controller reports status ABORTED
[ WARN] [1495858551.712792297, 46.274000000]: Controller handle rarm_controller reports status ABORTED
[ INFO] [1495858552.842772552, 47.401000000]: ABORTED: Solution found but controller failed during execution
[ INFO] [1495858563.887405916, 58.417000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1495858563.887508936, 58.417000000]: Planning attempt 1 of at most 1
[ INFO] [1495858563.888200496, 58.417000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1495858563.888251992, 58.417000000]: Planner configuration 'botharms[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1495858563.893137663, 58.423000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.893215300, 58.423000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.893369321, 58.423000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.893624850, 58.424000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.904997347, 58.435000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.905131406, 58.435000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.905223757, 58.435000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.905491018, 58.436000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.905647937, 58.436000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.017196 seconds
[ INFO] [1495858563.905916905, 58.436000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.905989619, 58.436000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.906060797, 58.436000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.906207396, 58.436000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.907849348, 58.437000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.907977891, 58.437000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.908649415, 58.437000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.908897658, 58.439000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.909027008, 58.439000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.003234 seconds
[ INFO] [1495858563.909216036, 58.439000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.909566976, 58.439000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858563.910454861, 58.440000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.911452985, 58.441000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858563.911791693, 58.442000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.002633 seconds
[ INFO] [1495858563.912582410, 58.443000000]: SimpleSetup: Path simplification took 0.000626 seconds and changed from 4 to 2 states
[ WARN] [1495858563.928549316, 58.459000000]: Controller handle larm_controller reports status ABORTED
[ INFO] [1495858566.933986055, 61.454000000]: ABORTED: Solution found but controller failed during execution
[ INFO] [1495858575.098244579, 69.600000000]: Clearing octomap...
[ INFO] [1495858575.098341450, 69.600000000]: Octomap cleared.
[ INFO] [1495858583.621893908, 78.108000000]: Replanning: yes
[ INFO] [1495858587.342768074, 81.822000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1495858587.342866091, 81.822000000]: Planning attempt 1 of at most 5
[ INFO] [1495858587.343869828, 81.823000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1495858587.343930444, 81.823000000]: Planner configuration 'botharms[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1495858587.344440848, 81.823000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.344578056, 81.824000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.344627579, 81.824000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.344787109, 81.824000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.357673657, 81.835000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.358049217, 81.835000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.359420213, 81.838000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (3 start + 2 goal)
[ INFO] [1495858587.361836278, 81.841000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.361994070, 81.841000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.017898 seconds
[ INFO] [1495858587.362260370, 81.841000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.362314002, 81.841000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.362382400, 81.841000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.362433827, 81.841000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.365222493, 81.844000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.365404790, 81.844000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.366007827, 81.845000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.366752426, 81.846000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.366855283, 81.846000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.004690 seconds
[ INFO] [1495858587.367000781, 81.846000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.367035952, 81.846000000]: botharms[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1495858587.368766406, 81.848000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.368882968, 81.848000000]: botharms[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [1495858587.369031826, 81.848000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.002082 seconds
[ INFO] [1495858587.370948247, 81.850000000]: SimpleSetup: Path simplification took 0.001842 seconds and changed from 4 to 2 states
[ WARN] [1495858590.530221951, 85.001000000]: Controller handle larm_controller reports status ABORTED
[ INFO] [1495858591.617845267, 86.084000000]: ABORTED: Solution found but controller failed during execution
^C[rviz_tork_kudu1_467_5789727499357910355-2] killing on exit
[move_group-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

1 participant