Dual UR5 Simulation

[wxmerkt]

Hi, There's a branch for the dual UR5 simulation (https://github.com/husky/husky_simulator/compare/dual_ur5_husky) but it doesn't seem to be completed and wasn't integrated. What's the timeline for this? We tried to get it to run ourselves but noticed a number of problems:

• It appears to rely on the added robotiq hand macro that isn't integrated upstream
• The FT300 isn't integrated into the upstream robotiq repository

In essence it's merging https://github.com/ros-industrial/robotiq/compare/jade-devel...TheDash:ft_300

Do you have updates on when and how this will be completed? Or instructions on how to run the dual_ur5_husky gazebo simulation?

Thanks, Wolfgang (Edinburgh)

[wxmerkt]

We've been using @TheDash's universal_robot, robotiq, and flir_ptu branches but we still cannot get the dual arm husky to load as shown on your twitter feed (https://twitter.com/clearpathrobots/status/746058485327085568) - would you mind providing us with step by step instructions for an installation on a new Trusty + Indigo setup?

Thanks, Wolfgang (Edinburgh)

[wxmerkt]

This is what it looks like and this is the log output:

wxm@wolf:~/dev/husky_ws$ roslaunch husky_gazebo husky_playpen.launch dual_ur5_enabled:=true robotiq_grippers_enabled:=true
... logging to /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/roslaunch-wolf-17307.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://127.0.0.1:40267/

SUMMARY
========

PARAMETERS
 * /ekf_localization/base_link_frame: base_link
 * /ekf_localization/frequency: 50
 * /ekf_localization/imu0: imu/data
 * /ekf_localization/imu0_config: [False, False, Fa...
 * /ekf_localization/imu0_differential: True
 * /ekf_localization/imu0_queue_size: 10
 * /ekf_localization/imu0_remove_gravitational_acceleration: True
 * /ekf_localization/odom0: husky_velocity_co...
 * /ekf_localization/odom0_config: [False, False, Fa...
 * /ekf_localization/odom0_differential: False
 * /ekf_localization/odom0_queue_size: 10
 * /ekf_localization/odom_frame: odom
 * /ekf_localization/two_d_mode: True
 * /ekf_localization/world_frame: odom
 * /flir_ptu_pan_controller/joint: husky_ptu_pan
 * /flir_ptu_pan_controller/pid/d: 0
 * /flir_ptu_pan_controller/pid/i: 0
 * /flir_ptu_pan_controller/pid/p: 0
 * /flir_ptu_pan_controller/type: position_controll...
 * /flir_ptu_tilt_controller/joint: husky_ptu_tilt
 * /flir_ptu_tilt_controller/pid/d: 0.0
 * /flir_ptu_tilt_controller/pid/i: 0
 * /flir_ptu_tilt_controller/pid/p: 0
 * /flir_ptu_tilt_controller/type: position_controll...
 * /husky_joint_publisher/publish_rate: 50
 * /husky_joint_publisher/type: joint_state_contr...
 * /husky_velocity_controller/angular/z/has_acceleration_limits: True
 * /husky_velocity_controller/angular/z/has_velocity_limits: True
 * /husky_velocity_controller/angular/z/max_acceleration: 6.0
 * /husky_velocity_controller/angular/z/max_velocity: 2.0
 * /husky_velocity_controller/base_frame_id: base_link
 * /husky_velocity_controller/cmd_vel_timeout: 0.25
 * /husky_velocity_controller/enable_odom_tf: False
 * /husky_velocity_controller/estimate_velocity_from_position: False
 * /husky_velocity_controller/left_wheel: ['front_left_whee...
 * /husky_velocity_controller/linear/x/has_acceleration_limits: True
 * /husky_velocity_controller/linear/x/has_velocity_limits: True
 * /husky_velocity_controller/linear/x/max_acceleration: 3.0
 * /husky_velocity_controller/linear/x/max_velocity: 1.0
 * /husky_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/publish_rate: 50
 * /husky_velocity_controller/right_wheel: ['front_right_whe...
 * /husky_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/type: diff_drive_contro...
 * /husky_velocity_controller/wheel_radius_multiplier: 1.0
 * /husky_velocity_controller/wheel_separation_multiplier: 1.875
 * /l_arm_controller/action_monitor_rate: 10
 * /l_arm_controller/constraints/goal_time: 0.6
 * /l_arm_controller/constraints/l_ur5_arm_elbow_joint/goal: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_elbow_joint/trajectory: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_shoulder_lift_joint/goal: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_shoulder_lift_joint/trajectory: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_shoulder_pan_joint/goal: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_shoulder_pan_joint/trajectory: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_wrist_1_joint/goal: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_wrist_1_joint/trajectory: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_wrist_2_joint/goal: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_wrist_2_joint/trajectory: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_wrist_3_joint/goal: 0.1
 * /l_arm_controller/constraints/l_ur5_arm_wrist_3_joint/trajectory: 0.1
 * /l_arm_controller/constraints/stopped_velocity_tolerance: 0.5
 * /l_arm_controller/joints: ['l_ur5_arm_shoul...
 * /l_arm_controller/state_publish_rate: 25
 * /l_arm_controller/stop_trajectory_duration: 0.5
 * /l_arm_controller/topic: left
 * /l_arm_controller/type: position_controll...
 * /r_arm_controller/action_monitor_rate: 10
 * /r_arm_controller/constraints/goal_time: 0.6
 * /r_arm_controller/constraints/r_ur5_arm_elbow_joint/goal: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_elbow_joint/trajectory: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_shoulder_lift_joint/goal: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_shoulder_lift_joint/trajectory: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_shoulder_pan_joint/goal: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_shoulder_pan_joint/trajectory: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_wrist_1_joint/goal: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_wrist_1_joint/trajectory: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_wrist_2_joint/goal: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_wrist_2_joint/trajectory: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_wrist_3_joint/goal: 0.1
 * /r_arm_controller/constraints/r_ur5_arm_wrist_3_joint/trajectory: 0.1
 * /r_arm_controller/constraints/stopped_velocity_tolerance: 0.5
 * /r_arm_controller/joints: ['r_ur5_arm_shoul...
 * /r_arm_controller/state_publish_rate: 25
 * /r_arm_controller/stop_trajectory_duration: 0.5
 * /r_arm_controller/topic: right
 * /r_arm_controller/type: position_controll...
 * /robot_description: <?xml version="1....
 * /robotiq_hands_l_hand_robot_state_publisher/publish_frequency: 50.0
 * /rosdistro: indigo
 * /rosversion: 1.11.20
 * /twist_mux/locks: [{'topic': 'e_sto...
 * /twist_mux/topics: [{'topic': 'joy_t...
 * /use_sim_time: True

NODES
  /
    base_controller_spawner (controller_manager/spawner)
    ekf_localization (robot_localization/ekf_localization_node)
    fake_joint_calibration (rostopic/rostopic)
    flir_ptu_pan_controller_spawner (controller_manager/spawner)
    flir_ptu_tilt_controller_spawner (controller_manager/spawner)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    left_arm_controller_spawner (controller_manager/spawner)
    right_arm_controller_spawner (controller_manager/spawner)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    robotiq_hands_l_hand_robot_state_publisher (robot_state_publisher/state_publisher)
    spawn_husky_model (gazebo_ros/spawn_model)
    stow_ur5_left (husky_control/stow_ur5_left)
    stow_ur5_right (husky_control/stow_ur5_right)
    twist_marker_server (interactive_marker_twist_server/marker_server)
    twist_mux (twist_mux/twist_mux)

ROS_MASTER_URI=http://127.0.0.1:11311

core service [/rosout] found
process[gazebo-1]: started with pid [17328]
process[gazebo_gui-2]: started with pid [17332]
process[robot_state_publisher-3]: started with pid [17336]
process[base_controller_spawner-4]: started with pid [17337]
process[ekf_localization-5]: started with pid [17338]
process[twist_marker_server-6]: started with pid [17344]
process[twist_mux-7]: started with pid [17353]
process[left_arm_controller_spawner-8]: started with pid [17364]
process[right_arm_controller_spawner-9]: started with pid [17377]
process[flir_ptu_pan_controller_spawner-10]: started with pid [17385]
process[flir_ptu_tilt_controller_spawner-11]: started with pid [17399]
process[fake_joint_calibration-12]: started with pid [17422]
process[robotiq_hands_l_hand_robot_state_publisher-13]: started with pid [17423]
process[spawn_husky_model-14]: started with pid [17428]
process[stow_ur5_left-15]: started with pid [17431]
process[stow_ur5_right-16]: started with pid [17435]
[ INFO] [1478353526.279014295]: [twist_marker_server] Initialized.
[ INFO] [1478353526.338660450]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1478353526.339188707]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1478353526.873044261, 1298.500000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1478353526.923363622, 1298.550000000]: Physics dynamic reconfigure ready.
[spawn_husky_model-14] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/spawn_husky_model-14*.log
[WARN] [WallTime: 1478353556.691638] [1328.000000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478353556.691661] [1328.000000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478353556.771724] [1328.080000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478353556.775622] [1328.080000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478353556.775631] [1328.080000] Controller Spawner couldn't find the expected controller_manager ROS interface.
Unhandled exception in thread started by 
sys.excepthook is missing
lost sys.stderr
Unhandled exception in thread started by 
sys.excepthook is missing
lost sys.stderr
Unhandled exception in thread started by 
sys.excepthook is missing
lost sys.stderr
[base_controller_spawner-4] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/base_controller_spawner-4*.log
[left_arm_controller_spawner-8] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/left_arm_controller_spawner-8*.log
[flir_ptu_pan_controller_spawner-10] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_pan_controller_spawner-10*.log
[flir_ptu_tilt_controller_spawner-11] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_tilt_controller_spawner-11*.log
[right_arm_controller_spawner-9] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/right_arm_controller_spawner-9*.log

[UltronDestroyer]

Hi @wxmerkt (@ibaranov-cp for insight)

Hi, There's a branch for the dual UR5 simulation (https://github.com/husky/husky_simulator/compare/dual_ur5_husky) but it doesn't seem to be completed and wasn't integrated. What's the timeline for this? We tried to get it to run ourselves but noticed a number of problems:

I was switched off the project soon as it shipped out the door. I'll try my best to see if I can help you, as I personally believe this is a badass robot setup.

It appears to rely on the added robotiq hand macro that isn't integrated upstream The FT300 isn't integrated into the upstream robotiq repository In essence it's merging ros-industrial/robotiq@jade-devel...TheDash:ft_300

That's right. I did not go through the motions to get it merged and ran out of time internally to work on the project. It should work though if you use my branch. Make sure you're using Indigo.

Do you have updates on when and how this will be completed? Or instructions on how to run the dual_ur5_husky gazebo simulation?

I believe there were also environment variables you had to set in addition to the parameters passed into the launch file.

This means in your bashrc or your terminal you need to have:

export FLIR_PTU_ENABLED=true export HUSKY_DUAL_UR5_ENABLED=true export ROBOTIQ_GRIPPERS_ENABLED=true export HUSKY_LASER_ENABLED=false export DUAL_ARM_BULKHEAD=true export HUSKY_TOP_PLATE_ENABLED=false

Try setting those and re-sourcing your environment and then launching it again. Let me know how it goes. I'll be online all day

Thanks, Wolfgang (Edinburgh)

In the future - I try to put all husky parameters here http://wiki.ros.org/husky_bringup/Tutorials/Customize%20Husky%20Configuration alongside their description

[wxmerkt]

This is awesome, thanks for your response and help @TheDash. Setting the environment variables makes the robot load in Gazebo, but the controllers seem to have an issue (not loading - couldn't be found) as well as the UR5s crashing through the ground as you've described in the universal_robot issue:

Related warning:

Warning [parser_urdf.cc:1231] multiple inconsistent <gravity> exists due to fixed joint reduction overwriting previous value [false] with [true].
[spawn_husky_model-14] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/spawn_husky_model-14*.log
[WARN] [WallTime: 1478356160.441292] [1325.650000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478356160.459170] [1325.670000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478356160.479225] [1325.690000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478356160.479571] [1325.690000] Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [WallTime: 1478356160.522252] [1325.730000] Controller Spawner couldn't find the expected controller_manager ROS interface.

I've installed ros-controllers from apt-get - do I need to install them from source?

Many thanks :)

[UltronDestroyer]

Double check you are using the"dual_ur5_husky" branch for the husky_simulator package as well

[wxmerkt]

Yes, using the dual_ur5_husky branch and this is the latest commit:

commit 26ee3d2401690e345ed62f5fc2c5ddb18b27c6fe
Author: DevonAsh <dash@clearpathrobotics.com>
Date:   Thu Jun 23 16:26:52 2016 +0000

    upd

[UltronDestroyer]

Thank you,

righteo, the .transmission.xacro files for the arms are included in the universal_robot package. Could you clone the universal_robot package from https://github.com/thedash/universal_robot

and compile it from source, double check that it is being used from source, and then give it a go?

In addition - you may also want to check that you have the specific type of ros controller installed. Gazebo uses the definitions in https://github.com/husky/husky/blob/dual_ur5_husky/husky_control/config/control_left_ur5.yaml to load the ros controller for each joint.

I would confirm that you have the plugins installed by:

sudo apt-get install ros-indigo-ros-control sudo apt-get install ros-indigo-gazebo-ros

[wxmerkt]

I'd already done that - and also removed the system-installed debians for ros-indigo-universal-robot, ros-indigo-ur-description, and ros-indigo-ur-kinematics - hence it should use it and pick the correct one up?

$ echo $ROS_PACKAGE_PATH
/home/wxm/dev/husky_ws/src:/opt/ros/indigo/share:/opt/ros/indigo/stacks
$ vcs remote
.....
=== ./flir_ptu (git) ===
origin  https://github.com/TheDash/flir_ptu.git (fetch)
origin  https://github.com/TheDash/flir_ptu.git (push)
=== ./husky (git) ===
origin  https://github.com/husky/husky.git (fetch)
origin  https://github.com/husky/husky.git (push)
=== ./husky_simulator (git) ===
origin  https://github.com/husky/husky_simulator.git (fetch)
origin  https://github.com/husky/husky_simulator.git (push)
=== ./robotiq (git) ===
origin  https://github.com/ros-industrial/robotiq (fetch)
origin  https://github.com/ros-industrial/robotiq (push)
thedash https://github.com/TheDash/robotiq (fetch)
thedash https://github.com/TheDash/robotiq (push)
=== ./universal_robot (git) ===
origin  https://github.com/TheDash/universal_robot.git (fetch)
origin  https://github.com/TheDash/universal_robot.git (push)
$ vcs branch
.....
=== ./flir_ptu (git) ===
fix-gazebo-errors
=== ./husky (git) ===
dual_ur5_husky
=== ./husky_simulator (git) ===
dual_ur5_husky
=== ./robotiq (git) ===
ft_300
=== ./universal_robot (git) ===
indigo-devel

[UltronDestroyer]

I'd already done that - and also removed the system-installed debians for ros-indigo-universal-robot, ros-indigo-ur-description, and ros-indigo-ur-kinematics - hence it should use it and pick the correct one up?

Okay, all of the above looks good. I edited the above to add a few more things - try those out too.

[wxmerkt]

Thanks - the problem was a missing gazebo package (silent failure, running Gazebo6.6 and soon to upgrade to 7). The Husky controllers are now loading, however, the Husky is half suspended in the ground and Gazebo crashes when running rosrun husky_control stow_ur5_left or immediately upon launching.

Truncated log:

Warning [parser_urdf.cc:1231] multiple inconsistent <gravity> exists due to fixed joint reduction overwriting previous value [false] with [true].
[ INFO] [1478358040.261061059, 1298.680000000]: Laser Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1478358040.261127546, 1298.680000000]: Starting Laser Plugin (ns = /)!
[ INFO] [1478358040.262749019, 1298.680000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1478358040.453321336, 1298.680000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1478358040.453571445, 1298.680000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1478358040.458486398, 1298.680000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1478358040.458869241, 1298.680000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1478358040.494218755, 1298.680000000]: Physics dynamic reconfigure ready.
[spawn_husky_model-14] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/spawn_husky_model-14*.log
[ INFO] [1478358040.804165831, 1298.680000000]: Loading gazebo_ros_control plugin
[ INFO] [1478358040.804328883, 1298.680000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1478358040.805148225, 1298.680000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ INFO] [1478358040.994067870, 1298.680000000]: Loaded gazebo_ros_control.
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
    if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 204, in main
    resp = switch_controller(loaded, [], 2)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
    raise ServiceException("transport error completing service call: %s"%(str(e)))
rospy.service.ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
    if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 190, in main
    resp = load_controller(name)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
    raise ServiceException("transport error completing service call: %s"%(str(e)))
rospy.service.ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
Traceback (most recent call last):
    Segmentation fault (core dumped)
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
    if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 204, in main
    if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 190, in main
if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 204, in main
        resp = switch_controller(loaded, [], 2)
    resp = load_controller(name)
resp = switch_controller(loaded, [], 2)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
            return self.call(*args, **kwds)
return self.call(*args, **kwds)
return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
            raise ServiceException("transport error completing service call: %s"%(str(e)))
raise ServiceException("transport error completing service call: %s"%(str(e)))
rospy.serviceraise ServiceException("transport error completing service call: %s"%(str(e)))
rospy.servicerospy.service..ServiceException.ServiceException: ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details: transport error completing service call: unable to receive data from sender, check sender's logs for detailstransport error completing service call: unable to receive data from sender, check sender's logs for details

[gazebo-1] process has died [pid 30573, exit code 139, cmd /opt/ros/indigo/lib/gazebo_ros/gzserver -e ode /home/wxm/dev/husky_ws/src/husky_simulator/husky_gazebo/worlds/clearpath_playpen.world __name:=gazebo __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/gazebo-1.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/gazebo-1*.log
[WARN] [WallTime: 1478358100.862989] [1298.940000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358100.871670] [1298.940000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358100.872962] [1298.940000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358100.874853] [1298.940000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358100.875958] [1298.940000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
Unhandled exception in thread started by 
sys.excepthook is missing
lost sys.stderr
[right_arm_controller_spawner-9] process has died [pid 30628, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner r_arm_controller --shutdown-timeout 3 __name:=right_arm_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/right_arm_controller_spawner-9.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/right_arm_controller_spawner-9*.log
Unhandled exception in thread started by 
sys.excepthook is missing
lost sys.stderr
Unhandled exception in thread started by 
sys.excepthook is missing
lost sys.stderr
[left_arm_controller_spawner-8] process has died [pid 30611, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner l_arm_controller --shutdown-timeout 3 __name:=left_arm_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/left_arm_controller_spawner-8.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/left_arm_controller_spawner-8*.log
[flir_ptu_pan_controller_spawner-10] process has died [pid 30640, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner flir_ptu_pan_controller --shutdown-timeout 3 __name:=flir_ptu_pan_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_pan_controller_spawner-10.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_pan_controller_spawner-10*.log
[flir_ptu_tilt_controller_spawner-11] process has died [pid 30644, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner flir_ptu_tilt_controller --shutdown-timeout 3 __name:=flir_ptu_tilt_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_tilt_controller_spawner-11.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_tilt_controller_spawner-11*.log
[base_controller_spawner-4] process has died [pid 30582, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner husky_joint_publisher husky_velocity_controller --shutdown-timeout 3 __name:=base_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/base_controller_spawner-4.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/base_controller_spawner-4*.log

Second log:

Warning [parser_urdf.cc:1231] multiple inconsistent <gravity> exists due to fixed joint reduction overwriting previous value [false] with [true].
[ INFO] [1478358323.589421176, 1298.680000000]: Laser Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1478358323.589485352, 1298.680000000]: Starting Laser Plugin (ns = /)!
[ INFO] [1478358323.590915635, 1298.680000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1478358323.788689497, 1298.680000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1478358323.789002843, 1298.680000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1478358323.796040526, 1298.680000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1478358323.798166602, 1298.680000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[spawn_husky_model-14] process has finished cleanly
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/spawn_husky_model-14*.log
[ INFO] [1478358324.272720379, 1298.680000000]: Loading gazebo_ros_control plugin
[ INFO] [1478358324.272866255, 1298.680000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1478358324.273728335, 1298.680000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
*** Error in `gzserver': double free or corruption (out): 0x00007f012ffdbd50 ***
Traceback (most recent call last):
Traceback (most recent call last):
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
    if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 190, in main
        resp = load_controller(name)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 190, in main
Traceback (most recent call last):
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
Aborted (core dumped)
        resp = load_controller(name)
    return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 505, in call
if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 190, in main
        resp = load_controller(name)
return self.call(*args, **kwds)
      File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
raise ServiceException("unable to connect to service: %s"%e)
rospy.service.ServiceException: unable to connect to service: [Errno 104] Connection reset by peer
    return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 505, in call
    raise ServiceException("unable to connect to service: %s"%e)
rospy.service.ServiceException:   File "/opt/ros/indigo/lib/controller_manager/spawner", line 212, in <module>
    if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 505, in call
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 190, in main
    unable to connect to service: [Errno 104] Connection reset by peer
    resp = load_controller(name)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    raise ServiceException("unable to connect to service: %s"%e)
rospy.service.ServiceException: unable to connect to service: [Errno 104] Connection reset by peer
    return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 505, in call
    raise ServiceException("unable to connect to service: %s"%e)
rospy.service.ServiceException: unable to connect to service: [Errno 104] Connection reset by peer
if __name__ == '__main__': main()
  File "/opt/ros/indigo/lib/controller_manager/spawner", line 190, in main
    resp = load_controller(name)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 505, in call
    raise ServiceException("unable to connect to service: %s"%e)
rospy.service.ServiceException: unable to connect to service: [Errno 104] Connection reset by peer
[gazebo-1] process has died [pid 12287, exit code 134, cmd /opt/ros/indigo/lib/gazebo_ros/gzserver -e ode /home/wxm/dev/husky_ws/src/husky_simulator/husky_gazebo/worlds/clearpath_playpen.world __name:=gazebo __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/gazebo-1.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/gazebo-1*.log
[WARN] [WallTime: 1478358328.671796] [1298.680000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358328.672088] [1298.680000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358328.672379] [1298.680000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358328.672753] [1298.680000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[WARN] [WallTime: 1478358328.673085] [1298.680000] Controller Spawner couldn't reach controller_manager to take down controllers. Waited for 3 second(s).
[base_controller_spawner-4] process has died [pid 12296, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner husky_joint_publisher husky_velocity_controller --shutdown-timeout 3 __name:=base_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/base_controller_spawner-4.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/base_controller_spawner-4*.log
[right_arm_controller_spawner-9] process has died [pid 12331, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner r_arm_controller --shutdown-timeout 3 __name:=right_arm_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/right_arm_controller_spawner-9.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/right_arm_controller_spawner-9*.log
[flir_ptu_pan_controller_spawner-10] process has died [pid 12346, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner flir_ptu_pan_controller --shutdown-timeout 3 __name:=flir_ptu_pan_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_pan_controller_spawner-10.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_pan_controller_spawner-10*.log
[flir_ptu_tilt_controller_spawner-11] process has died [pid 12347, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner flir_ptu_tilt_controller --shutdown-timeout 3 __name:=flir_ptu_tilt_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_tilt_controller_spawner-11.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/flir_ptu_tilt_controller_spawner-11*.log
[left_arm_controller_spawner-8] process has died [pid 12324, exit code 1, cmd /opt/ros/indigo/lib/controller_manager/spawner l_arm_controller --shutdown-timeout 3 __name:=left_arm_controller_spawner __log:=/home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/left_arm_controller_spawner-8.log].
log file: /home/wxm/.ros/log/6df40358-a35c-11e6-a5d5-5cc5d49636d3/left_arm_controller_spawner-8*.log

[wxmerkt]

Just to make sure - am I launching it correctly?:

roslaunch husky_gazebo husky_playpen.launch dual_ur5_enabled:=true robotiq_grippers_enabled:=true

[UltronDestroyer]

Does it show up in Gazebo with the arms fine now? What is the bumblebee doing?

roslaunch husky_gazebo husky_playpen.launch dual_ur5_enabled:=true robotiq_grippers_enabled:=true laser_enabled:=false

I would give that a go. Sorry my answers aren't more certain, been a while since I've done this robot.

The above looks like you just need to reset your terminals - gazebo didn't properly free its memory - OR that there are incompatible versions of gazebo-ros-pluginns and gazebo going on here. I think I used Gazebo 2 for all of this.

[wxmerkt]

Thanks, I've got it to start and the controllers to load. The simulation is unstable though, the robot is rocking and slowly moving backwards when nothing is commanded. I'll investigate this further - it appears to be some tuning issue of the physics solver.

The FT sensors do not show up though. I've also exported ROBOTIQ_FT_300_ENABLED=true now, they still do not come up (e.g. in rviz). Any idea why?

Also, there are inconsistencies between the visualised state in gazebo and the onein rviz, cf. this video:

https://www.youtube.com/watch?v=r3NYTo1KBUY&feature=youtu.be

[UltronDestroyer]

Thanks, I've got it to start and the controllers to load. The simulation is unstable though, the robot is rocking and slowly moving backwards when nothing is commanded. I'll investigate this further - it appears to be some tuning issue of the physics solver.

Quite strange. I've seen small jitters/movements for things like this when inertia values aren't set correctly, or control loops aren't tuned. Could also be the friction in the physics.

The FT sensors do not show up though. I've also exported ROBOTIQ_FT_300_ENABLED=true now, they still do not come up (e.g. in rviz). Any idea why?

Hmm.. https://github.com/husky/husky/blob/dual_ur5_husky/husky_description/urdf/husky.urdf.xacro#L168 says they're there. Could you check your robot_description and the links/joints in Rviz? I think it is there. I put such a big space between the gripper and the arm because the real robot has some sort of spacing/attachment material there that isn't the FT. I never got around to adding a plain cylinder in place for the sim, so just made a gap.

Im thinking the physics values of the Husky are wrong. It loooks like it needs more weight in the front.

Side note: what are you using for navigation? This whole setup is pretty awesome.

[wxmerkt]

Quite strange. I've seen small jitters/movements for things like this when inertia values aren't set correctly, or control loops aren't tuned. Could also be the friction in the physics.

Yes, that may be it. I didn't look into it too closely just yet until I can resolve the weight distribution issue.

Hmm.. https://github.com/husky/husky/blob/dual_ur5_husky/husky_description/urdf/husky.urdf.xacro#L168 says they're there. Could you check your robot_description and the links/joints in Rviz?

Correct, it's there and it finds the link frames successfully. It has trouble finding a transform from any of the hand frames to the odom frame though. Any idea what might be causing that?

Im thinking the physics values of the Husky are wrong. It loooks like it needs more weight in the front.

Do you have any particular insight on what weight needs to be adjusted where? I.e. what was the mass of the robot for shipping so we can compare it to the one in the URDF (e.g. the battery weight might be unaccounted for or perhaps the weight of the UR5 control computer).

Side note: what are you using for navigation? This whole is pretty awesome.

It's the plain husky_navigation stack which comes as part of husky_desktop.

[UltronDestroyer]

Correct, it's there and it finds the link frames successfully. It has trouble finding a transform from any of the hand frames to the odom frame though. Any idea what might be causing that?

Not entirely sure. roswtf or rosrun tf view_frames would help here to see if there are any tf complaints.

Do you have any particular insight on what weight needs to be adjusted where? I.e. what was the mass of the robot for shipping so we can compare it to the one in the URDF (e.g. the battery weight might be unaccounted for or perhaps the weight of the UR5 control computer).

Might have to wait until Monday for this one. @jeff-o any insight on how we could find that out?

It's the plain husky_navigation stack which comes as part of husky_desktop.

I don't get out much :P

[wxmerkt]

Not entirely sure. roswtf or rosrun tf view_frames would help here to see if there are any tf complaints.

There's no connection to the grippers from the _palm - is it possible it's not being simulated properly? Cf. frames.pdf

[UltronDestroyer]

Hmm. Can you check the robot_description to see if the fingers are defined there? It is possible that the gazebo tags that are in the robotiq_hand macro don't work on this version of gazebo and so the model wasn't added to the srdf.

Edit: I take that back. http://gazebosim.org/tutorials?tut=ros_urdf says anything 1.9+ above these tags should work.

It seems the fingers aren't being loaded but the palm is.

[wxmerkt]

The only ROS node with "Robotiq" in it is a joint state publisher, I haven't seen a controller for it running:

$ rosservice call /controller_manager/list_controllers
controller: 
  - 
    name: flir_ptu_tilt_controller
    state: running
    type: position_controllers/JointPositionController
    hardware_interface: hardware_interface::PositionJointInterface
    resources: ['husky_ptu_tilt']
  - 
    name: husky_joint_publisher
    state: running
    type: joint_state_controller/JointStateController
    hardware_interface: hardware_interface::JointStateInterface
    resources: []
  - 
    name: l_arm_controller
    state: running
    type: position_controllers/JointTrajectoryController
    hardware_interface: hardware_interface::PositionJointInterface
    resources: ['l_ur5_arm_elbow_joint', 'l_ur5_arm_shoulder_lift_joint', 'l_ur5_arm_shoulder_pan_joint', 'l_ur5_arm_wrist_1_joint', 'l_ur5_arm_wrist_2_joint', 'l_ur5_arm_wrist_3_joint']
  - 
    name: husky_velocity_controller
    state: running
    type: diff_drive_controller/DiffDriveController
    hardware_interface: hardware_interface::VelocityJointInterface
    resources: ['front_left_wheel', 'front_right_wheel', 'rear_left_wheel', 'rear_right_wheel']
  - 
    name: flir_ptu_pan_controller
    state: running
    type: position_controllers/JointPositionController
    hardware_interface: hardware_interface::PositionJointInterface
    resources: ['husky_ptu_pan']
  - 
    name: r_arm_controller
    state: running
    type: position_controllers/JointTrajectoryController
    hardware_interface: hardware_interface::PositionJointInterface
    resources: ['r_ur5_arm_elbow_joint', 'r_ur5_arm_shoulder_lift_joint', 'r_ur5_arm_shoulder_pan_joint', 'r_ur5_arm_wrist_1_joint', 'r_ur5_arm_wrist_2_joint', 'r_ur5_arm_wrist_3_joint']

[UltronDestroyer]

Oh yes. that is right. It has no controller for it and has its own controller that must be loaded separately. Good find.

http://wiki.ros.org/robotiq/Tutorials/Control%20of%20an%20S-Model%20Gripper%20using%20the%20Modbus%20TCP%20Protocol

Try bullet point 2. I'll keep looking, because I feel like I wrote an internal reference for how to use the gripper.

There was a mini readme at https://github.com/husky/husky/tree/dual_ur5_husky if you scroll down it says:

1) Double check the gripper drivers are running

rostopic list | grep gripper 2) Launch the simple gripper interface

rosrun robotiq_s_model_control SModelSimpleController.py 3) For more complex control, see this http://wiki.ros.org/robotiq/Tutorials/Control%20of%20an%20S-Model%20Gripper%20using%20the%20Modbus%20TCP%20Protocol

The reason being that it was so complex (because it is underactuated) that Gazebo + ROS Control wasn't suited for it - so it was just best to use the alternative controller and tap into that.

Edit: I think that may be for the real robot. http://gazebosim.org/tutorials?tut=drcsim_robotiq_hand&cat=drcsim is the reference for controlling it

https://github.com/ros-industrial/robotiq/blob/groovy-devel/robotiq_s_model_articulated_gazebo/launch/robotiq_gripper_empty_world.launch was merged once as a reference but they never carried forward the changes I made, so it was lost.

[wxmerkt]

Try bullet point 2. I'll keep looking, because I feel like I wrote an internal reference for how to use the gripper.

What would those IPs be for the simulated version? We got it working just fine on the real robot.

Priority for us now is to get the mass distribution of the robot correct though so that we can drive around in simulation.

Thank you very much for all your help and advice, this is awesome :)

[UltronDestroyer]

What would those IPs be for the simulated version? We got it working just fine on the real robot.

Priority for us now is to get the mass distribution of the robot correct though so that we can drive around in simulation.

Thank you very much for all your help and advice, this is awesome :)

No IPs for simulated version. I believe the grippers move in simulation using direct joint state publisher commands akin to how it is done with the Atlas (which use robotiq grippers)

http://gazebosim.org/tutorials?tut=drcsim_robotiq_hand&cat=drcsim

In particular:

rostopic pub --once right_hand/command atlas_msgs/SModelRobotOutput {1,0,1,0,0,0,127,255,0,155,0,0,255,0,0,0,0,0}

is the jist of the command, but you may need to change topic names from right_hand/command to whatever it shows up as on the Husky.

[UltronDestroyer]

@wxmerkt

Is there an outstanding item on this ticket we haven't addressed? I would like to make sure everything is all good.

[wxmerkt]

Hey @TheDash, Thanks for checking in - the Husky still isn't navigating/driving without performing a wheelie in simulation. I believe this is due to the default acceleration/velocities for ROS Navigation in planning.yaml which we are investigating for the physical robot as well (too high). Do you have a recommendation for a sane/safe value? Experimentally we see 0.2m/s linear and 0.1m/s angular as okay values for velocity, any hunch on acceleration?

[tonybaltovski]

@wxmerkt any updates on this issue?

[wxmerkt]

We haven't used Gazebo much any further and cannot confirm whether it's working or not.

Our real Husky has (as confirmed by the maintenance visit of @majcote) a defective motor driver reporting erroneous values thus preventing usage - we are still waiting to hear back on a) a fix of that (right side) motor driver and b) merging of the differential drive controller with velocity input rather than position.

cc: @VladimirIvan