Disabling Gazebo Baxter causes unexpected arm movements

[rethink-imcmahon]

As reported by @dkretzsc:

"I can launch the simulator by going to my ros_ws workspace and using ./baxter.sh sim and the command roslaunch baxter_gazebo baxter_world.launch.

I wait until I see the three lines:

[ INFO] [1400513321.531488283, 34.216000000]: Simulator is loaded and started successfully
[ INFO] [1400513321.535040726, 34.219000000]: Robot is disabled
[ INFO] [1400513321.535125386, 34.220000000]: Gravity compensation was turned off

In another terminal, I go to Baxter's ros_ws, enter ./baxter.sh sim, and then launch the baxter example

rosrun baxter_examples joint_velocity_wobbler.py. 

The example runs on the simulator, but when I enter Ctrl c to exit, the terminal states that the robot
is entering a neutral pose and then disabled; however, the simulator shows that the arms are still
moving/flaying about in snake like movements and end in very unnatural poses for Baxter.

I want to use the simulator to test code that I write for Baxter but am concerned that I cannot even get valid example code to correctly run.

I put in a text file, the output from the two terminals I used to run the example and also used a screenshot of a pose that the simulator Baxter is in after he is theoretically in neutral pose and has been disabled.

I have tried troubleshooting by looking at the forums and also the troubleshooting page with no success. (I looked at the discussion at this link:
https://groups.google.com/a/rethinkrobotics.com/forum/#!searchin/brr-users/simulator/brr-users/LXIjjASQPGw/wMb7aV-A8l8J

However, I did not have an old copy of gazebo."

A screencap of the end result:

[rethink-imcmahon]

the output of roslaunch baxter_gazebo baxter_world.launch:

[INFO] [WallTime: 1441031377.217535] [2.130000] Controller Spawner: Loaded controllers:
left_joint_position_controller, right_joint_position_controller, head_position_controller,
left_joint_velocity_controller, right_joint_velocity_controller, left_joint_effort_controller,
right_joint_effort_controller
[ INFO] [1441031410.057831244, 34.842000000]: Simulator is loaded and started successfully
[ INFO] [1441031410.060853555, 34.845000000]: Robot is disabled
[ INFO] [1441031410.061055467, 34.845000000]: Gravity compensation was turned off
[ INFO] [1441031503.857898280, 128.313000000]: Robot is enabled
[ INFO] [1441031503.858229537, 128.313000000]: left_joint_position_controller was started and
left_joint_velocity_controller and left_joint_effort_controller were stopped succesfully
[ INFO] [1441031503.858407702, 128.313000000]: Gravity compensation was turned on
[ INFO] [1441031507.885172768, 132.323000000]: Robot is enabled
[ INFO] [1441031507.885260558, 132.323000000]: right_joint_position_controller was started
and right_joint_velocity_controller and right_joint_effort_controller were stopped succesfully
[ INFO] [1441031507.885302812, 132.323000000]: Gravity compensation was turned on
[ INFO] [1441031511.977574113, 136.401000000]: Robot is enabled
[ INFO] [1441031511.977644583, 136.401000000]: left_joint_velocity_controller was started and
left_joint_position_controller and left_joint_effort_controller were stopped succesfully
[ INFO] [1441031511.977682646, 136.401000000]: Gravity compensation was turned on
[ INFO] [1441031511.978626970, 136.403000000]: Robot is enabled
[ INFO] [1441031511.978680609, 136.403000000]: right_joint_velocity_controller was started
and right_joint_position_controller and right_joint_effort_controller were stopped succesfully
[ INFO] [1441031511.978739694, 136.403000000]: Gravity compensation was turned on
[ INFO] [1441031542.951571226, 167.233000000]: Robot is enabled
[ INFO] [1441031542.951655455, 167.233000000]: left_joint_position_controller was started and
left_joint_velocity_controller and left_joint_effort_controller were stopped succesfully
[ INFO] [1441031542.951696033, 167.233000000]: Gravity compensation was turned on
[ INFO] [1441031542.952570515, 167.234000000]: Robot is enabled
[ INFO] [1441031542.952664941, 167.234000000]: right_joint_position_controller was started
and right_joint_velocity_controller and right_joint_effort_controller were stopped succesfully
[ INFO] [1441031542.952734223, 167.234000000]: Gravity compensation was turned on
[ INFO] [1441031549.380433863, 173.636000000]: Robot is disabled
[ INFO] [1441031549.380504822, 173.636000000]: Gravity compensation was turned off

[rethink-imcmahon]

the output of rosrun baxter_examples joint_velocity_wobbler.py:

~/ros_ws$ ./baxter.sh sim
~/ros_ws$ rosrun baxter_examples joint_velocity_wobbler.py
[INFO] [WallTime: 1441031503.775147] [128.230000] Robot Enabled
[INFO] [WallTime: 1441031549.388135] [173.643000] Robot Disabled
File "/home/baxter/ros_ws/src/baxter_examples/scripts/joint_velocity_wobbler.py", line 158, in
File "/home/baxter/ros_ws/src/baxter_examples/scripts/joint_velocity_wobbler.py", line 153, in
File "/home/baxter/ros_ws/src/baxter_examples/scripts/joint_velocity_wobbler.py", line 134, in
File "/opt/ros/indigo/lib/python2.7/dist­packages/rospy/timer.py", line 99, in sleep
File "/opt/ros/indigo/lib/python2.7/dist­packages/rospy/timer.py", line 157, in sleep
raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request

[rethink-imcmahon]

First off, the ROS shutdown requested exception should never be thrown in joint_velocity_wobbler.py which points to a problem with how we handle disabling the simulated robot. It must differ somehow from how we handle it on the real robot.

[rethink-imcmahon]

I managed to get a different error on velocity wobbler exit:

$ rosrun baxter_examples joint_velocity_wobbler.py 
Initializing node... 
Getting robot state... 
Enabling robot... 
[INFO] [WallTime: 1441731848.427393] [7726.313000] Robot Enabled
Moving to neutral pose...
Left Moving to neutral pose complete...
Right Moving to neutral pose complete...
Wobbling. Press Ctrl-C to stop...
^C
Exiting example...
Moving to neutral pose...
Left Moving to neutral pose complete...
Right Moving to neutral pose complete...
Disabling robot...
[INFO] [WallTime: 1441731868.003786] [7745.839000] Robot Disabled
Finished shutdown check...
Traceback (most recent call last):
  File "/data/users/imcmahon/dev/mrsp_ws/src/baxter_examples/scripts/joint_velocity_wobbler.py", line 164, in <module>
    main()
  File "/data/users/imcmahon/dev/mrsp_ws/src/baxter_examples/scripts/joint_velocity_wobbler.py", line 159, in main
    wobbler.wobble()
  File "/data/users/imcmahon/dev/mrsp_ws/src/baxter_examples/scripts/joint_velocity_wobbler.py", line 136, in wobble
    self._right_arm.set_joint_velocities(cmd)
  File "/data/users/imcmahon/dev/mrsp_ws/src/baxter_interface/src/baxter_interface/limb.py", line 367, in set_joint_velocities
    self._pub_joint_cmd.publish(self._command_msg)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 819, in publish
    self.impl.publish(data)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 999, in publish
    b.tell()
AttributeError: 'NoneType' object has no attribute 'tell'

I have a feeling this is a rospy issue with simulated time, but no proof as of yet.

[rethink-imcmahon]

It seems odd that the while loop of baxter_examples/joint_velocity_wobbler.py

130         while not rospy.is_shutdown():
131             self._pub_rate.publish(self._rate)
132             elapsed = rospy.Time.now() - start
133             cmd = make_cmd(self._left_joint_names, elapsed)
134             self._left_arm.set_joint_velocities(cmd)
135             cmd = make_cmd(self._right_joint_names, elapsed)
136             self._right_arm.set_joint_velocities(cmd)
137             rate.sleep()

continues to execute, despite ctrl+c being evoked. This only happens on the simulated robot, not the real one.

[rethink-imcmahon]

@dkretzsc I suspect the difference between this example's behavior is a problem with rospy and simulated time. I have made the joint_velocity_wobbler handle its own signals, and manually call rospy.shutdown after cleanup is complete. This prevents all rospy errors in the simulator, and works on the real robot. You can view the change here: RethinkRobotics/baxter_examples#49
However, I think I have fixed the red herring issue. rospy no longer errors out in the middle of shutdown, but there is still an issue with Baxter's arms drifting randomly whenever it is disabled.

[davetcoleman]

I have encountered this same issue in Gazebo7 just now