rethinkrobotics/sawyer_simulator

Gazebo Simulation interface for the Sawyer Robot

sawyer_simulator's Issues

Cuff interface needs to be simulated in joint_trajectory_action.py

joint_trajectory_action.py depends on the robot Cuff interface in that it loads and checks the state of the Cuff during its command actions. There is currently no simulated ROS interface nor published topics in the simulator for this device, which causes the Cuff to fail to load, thus causing JTAS to fail to load, thus causing MoveIt to have problems when run against Gazebo.

In general, there might be a handful of minor interfaces, which are not relevant to the simulated robot and therefore do not have published topics in the sim launch, but cause hiccups b/c they are included in various examples.

Add collision checking to Inverse Kinematics Sim Service

if (jointsInCollision(kinematic_chain_map_[req.tip_names[i]], jnt_result))
        {
          res.result_type[i] = res.IK_IN_COLLISION;
        }  TODO(imcmahon) Utilize FCL for collision checking

We could use FCL, or some other library to check to see if the robot is in collision at any configuration.

Improve README.md docs

intera_core_msgs dependency issue in sawyer_sim_controllers

When following directions on Gazebo Tutorial I'm greeted with the following error:

[  1%] Building CXX object sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_robot_hw_sim.dir/src/sawyer_robot_hw_sim.cpp.o
In file included from /home/jarvis/52sawyerws/src/sawyer_simulator/sawyer_sim_controllers/src/sawyer_velocity_controller.cpp:17:0:
/home/jarvis/52sawyerws/src/sawyer_simulator/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_velocity_controller.h:21:43: fatal error: intera_core_msgs/JointCommand.h: No such file or directory
compilation terminated.
sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/build.make:86: recipe for target 'sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_velocity_co
ntroller.cpp.o' failed
make[2]: *** [sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_velocity_controller.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
Scanning dependencies of target intera_examples_gen

This indicates catkin is attempting to build the sawyer_robot_hw_sim.cpp.o target before the intera_core_msgs/JointCommand.msg target has been built. Likely need to explicitly add a dependency on ${catkin_EXPORTED_TARGETS} for this lib.

Noetic development branch

Per the instructions on https://sdk.rethinkrobotics.com/intera/Gazebo_Tutorial there should be a branch called noetic_devel in this repo. However, it does not look like it has been created yet.

Is support for ROS Noetic planned?

We are looking for a way to use Python 3 and simulate Sawyer in Gazebo - if there is some other way to do this, it would be nice to know.

Question? I get something wrong with protobuf when I roslaunch files?

Thanks for your code~
Here I get some question, when I run roslaunch sawyer_gazebo sawyer_world.launch, it shows that This program requires version 3.6.1 of the Protocol Buffer runtime library, but the installed version is 2.6.1. Please update your library.
But in my computer (ubuntu16 and ros kinetic), the version of protobuf is 2.6, and when I change it with conda( protobuf 3.6), still same question ? can you help me ?

In contrast, I run well in roslaunch baxter_gazebo baxter_world.launch.

NODES
  /io/internal_camera/head_camera/
    rectify_color (nodelet/nodelet)
  /io/internal_camera/right_hand_camera/
    rectify_mono (nodelet/nodelet)
  /
    camera_sim (sawyer_gazebo/cameras_sim_io_node.py)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    intera_camera_nodelets (nodelet/nodelet)
    io_robot (rosbag/play)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    urdf_spawner (gazebo_ros/spawn_model)
  /robot/
    controller_spawner (controller_manager/controller_manager)
    controller_spawner_stopped (controller_manager/controller_manager)
    electric_gripper_controller_spawner_stopped (controller_manager/controller_manager)

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

setting /run_id to b7091e44-5150-11e9-80cd-dca266407807
process[rosout-1]: started with pid [26809]
started core service [/rosout]
process[gazebo-2]: started with pid [26812]
process[gazebo_gui-3]: started with pid [26813]
process[urdf_spawner-4]: started with pid [26815]
process[robot/controller_spawner-5]: started with pid [26817]
process[robot/controller_spawner_stopped-6]: started with pid [26818]
process[robot/electric_gripper_controller_spawner_stopped-7]: started with pid [26819]
process[robot_state_publisher-8]: started with pid [26820]
process[camera_sim-9]: started with pid [26821]
process[intera_camera_nodelets-10]: started with pid [26864]
process[io/internal_camera/right_hand_camera/rectify_mono-11]: started with pid [26879]
process[io/internal_camera/head_camera/rectify_color-12]: started with pid [26886]
process[io_robot-13]: started with pid [26899]
SpawnModel script started
[INFO] [1553774275.739454, 0.000000]: Loading model XML from ros parameter
[INFO] [1553774275.752054, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1553774275.925415637]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1553774275.925742845]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1553774275.925793775]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1553774275.926163319]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[INFO] [1553774277.264633, 0.000000]: Calling service /gazebo/spawn_urdf_model
[ INFO] [1553774277.808966833, 0.001000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1553774277.811543157, 0.001000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1553774277.811698448, 0.001000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1553774277.813304356, 0.001000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[INFO] [1553774277.816968, 0.001000]: Spawn status: SpawnModel: Successfully spawned entity
[INFO] [1553774277.817247, 0.001000]: Waiting for service /gazebo/set_model_configuration
[INFO] [1553774277.818196, 0.001000]: temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition.
[ INFO] [1553774278.408524102, 0.001000000]: Loading gazebo_ros_control plugin
[ INFO] [1553774278.413633583, 0.001000000]: Starting gazebo_ros_control plugin in namespace: /robot
[ INFO] [1553774278.414375196, 0.001000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [/robot_description] on the ROS param server.
[INFO] [1553774278.819395, 0.001000]: Calling service /gazebo/set_model_configuration
[INFO] [1553774278.824308, 0.001000]: Set model configuration status: SetModelConfiguration: success
[urdf_spawner-4] process has finished cleanly
log file: /home/zzl/.ros/log/b7091e44-5150-11e9-80cd-dca266407807/urdf_spawner-4*.log
[libprotobuf FATAL google/protobuf/stubs/common.cc:61] This program requires version 3.6.1 of the Protocol Buffer runtime library, but the installed version is 2.6.1.  Please update your library.  If you compiled the program yourself, make sure that your headers are from the same version of Protocol Buffers as your link-time library.  (Version verification failed in "google/protobuf/any.pb.cc".)
terminate called after throwing an instance of 'google::protobuf::FatalException'
  what():  This program requires version 3.6.1 of the Protocol Buffer runtime library, but the installed version is 2.6.1.  Please update your library.  If you compiled the program yourself, make sure that your headers are from the same version of Protocol Buffers as your link-time library.  (Version verification failed in "google/protobuf/any.pb.cc".)
Aborted
[gazebo-2] process has died [pid 26812, exit code 134, cmd /opt/ros/kinetic/lib/gazebo_ros/gzserver -e ode /home/zzl/RL/gym_ros/installation/baxter_ws/src/sawyer/sawyer_simulator/sawyer_gazebo/worlds/sawyer.world __name:=gazebo __log:=/home/zzl/.ros/log/b7091e44-5150-11e9-80cd-dca266407807/gazebo-2.log].
log file: /home/zzl/.ros/log/b7091e44-5150-11e9-80cd-dca266407807/gazebo-2*.log

Invalid endpoint_state and FK

Hi!
First of all, say that this happens in Ubuntu 18.04 with melodic, which I am not sure it's supported. I tried with a 16.04 vm and gazebo segfaults so I could not verify whether the issue is there too.
The simulation in 18.04 works well (there's one missing joint in the wrist, but rviz displays it properly).

The issue I am observing is that the endpoint_state topic /robot/limb/right/endpoint_state is always providing invalid poses, which are (0,0,0). I tracked this down to ArmKinematicsInterface::computePositionFK, where it seems that the KDL's JntToCart returns an invalid solution. I'm not familiar with KDL.

I find it quite strange that it's FK that fails (was expecting anything to not work but this). I am using the melodic-devel branch from sns_ik, although I guess that's not related to sns_ik.
Moreover, all TFs seem correct in rviz.

I am not sure about what can be going on, or whether it's KDL's, sawyer_simulator or if it's just not supported yet in 18.04.

Error when running Simulated Pick and Place example

I installed everything required in accordance with
https://sdk.rethinkrobotics.com/intera/Workstation_Setup
https://sdk.rethinkrobotics.com/intera/Gazebo_Tutorial

And tried to run Simulated Pick and Place example with command:
roslaunch sawyer_sim_examples sawyer_pick_and_place_demo.launch
I constantly getting next error

[ERROR] [1588181573.692278, 5.025000]: INVALID POSE - No Valid Joint Solution Found.
[ERROR] [1588181573.692531, 5.025000]: No Joint Angles provided for move_to_joint_positions. Staying put.
[ERROR] [1588181573.701628, 5.034000]: INVALID POSE - No Valid Joint Solution Found.
[ERROR] [1588181573.702054, 5.035000]: No Joint Angles provided for move_to_joint_positions. Staying put.

Logs folder created after running:
logs.zip

In Gazebo arm is moving, trying to take red box, but every time misses to take it. "Claws" going down already closed.

How can I fix it?

Could not find package configuration file provided by "hardware_interface"

CMake Warning at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:76 (find_package):
Could not find a package configuration file provided by
"hardware_interface" with any of the following names:

hardware_interfaceConfig.cmake
hardware_interface-config.cmake

Add the installation prefix of "hardware_interface" to CMAKE_PREFIX_PATH or
set "hardware_interface_DIR" to a directory containing one of the above
files. If "hardware_interface" provides a separate development package or
SDK, be sure it has been installed.
Call Stack (most recent call first):
sawyer_simulator/sawyer_hardware_interface/CMakeLists.txt:7 (find_package)

-- Could not find the required component 'hardware_interface'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by
"hardware_interface" with any of the following names:

hardware_interfaceConfig.cmake
hardware_interface-config.cmake

Add the installation prefix of "hardware_interface" to CMAKE_PREFIX_PATH or
set "hardware_interface_DIR" to a directory containing one of the above
files. If "hardware_interface" provides a separate development package or
SDK, be sure it has been installed.
Call Stack (most recent call first):
sawyer_simulator/sawyer_hardware_interface/CMakeLists.txt:7 (find_package)

-- Configuring incomplete, errors occurred!
See also "/home/psuresh/catkin_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/psuresh/catkin_ws/build/CMakeFiles/CMakeError.log".
Invoking "cmake" failed

Intera behavior editor with simulator

Hi,
Had a quick chat with Dr Brooks at ICRA 2018 today and thought will drop in a line here. We have baxters in our lab but no sawyer. However, I am quite keen to try out the intera studio editor in ROS+gazebo(http://mfg.rethinkrobotics.com/intera/Behavior_Editor_Overview). wondering if that is possible?

[Crashes!] Gazebo 7 simulation crashes!

Description

• The gazebo crashes in my RL training process for sawyer.
• After I found the minimal code to reproduce the crash, I found that the crash happens when I repeatedly call set_joint_positions and rospy.wait_for_message(...). And the crash will happen after about 1500 iterations.
• The minimal code to reproduce the crash is as follows:

import rospy
import intera_interface
from gazebo_msgs.msg import (
    ModelStates
)

class BugReproducer(object):
    def __init__(self, limb="right", hover_distance = 0.15, tip_name="right_gripper_tip"):
        self._limb = intera_interface.Limb(limb)
        self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
        self._rs.enable()

    def clean_shutdown(self):
        print("Exiting...")
        if self._rs.state().enabled:
            print("Disabling robot...")
            self._rs.disable()

rospy.init_node("bug_reproducer")
br = BugReproducer()
rospy.on_shutdown(br.clean_shutdown)
joint_angles = {'right_j0': -0.041662954890248294,
                'right_j1': -1.0258291091425074,
                'right_j2': 0.0293680414401436,
                'right_j3': 2.17518162913313,
                'right_j4':  -0.06703022873354225,
                'right_j5': 0.3968371433926965,
                'right_j6': 1.7659649178699421}
while True:
    br._limb.set_joint_positions(joint_angles)
    rospy.wait_for_message("/gazebo/model_states", ModelStates, timeout=5)

• And the backtrace of the crash is:

#0  gazebo::physics::World::Update (this=this@entry=0x10cd580)
    at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/physics/World.cc:740
#1  0x00007ffff62d89af in gazebo::physics::World::Step (this=this@entry=0x10cd580)
    at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/physics/World.cc:672
#2  0x00007ffff62d8e25 in gazebo::physics::World::RunLoop (this=0x10cd580)
    at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/physics/World.cc:481
#3  0x00007ffff40d85d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#4  0x00007ffff65ed6ba in start_thread (arg=0x7fff377fe700) at pthread_create.c:333
#5  0x00007ffff6bef41d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

Environment Parameters

• Operating System Version: Ubuntu 16.04
• ROS Version: Kinetic
• Gazebo Version: Gazebo 7.0.0

Create Cuff IO Sim

To properly run the Joint Trajectory Action Server for use with MoveIt, there needs to be a sim IO script.

what is it in simulation?what is its function?

./intera.sh sim ,for simulation,what is it?

[ERROR] Controller 'head_position_controller' is already running

We need to stop trying to start the head_position_controller twice (causing one to error out).

extend the support for Ignition gazebo

Hello there,

How to use this for simulation in ignition gazebo?

Compile error when use catkin_make in ubuntu16.04 (Kinetic)

c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See file:///usr/share/doc/gcc-5/README.Bugs for instructions.
sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_robot_hw_sim.dir/build.make:62: recipe for target 'sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_robot_hw_sim.dir/src/sawyer_robot_hw_sim.cpp.o' failed
make[2]: *** [sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_robot_hw_sim.dir/src/sawyer_robot_hw_sim.cpp.o] Error 4
CMakeFiles/Makefile2:6920: recipe for target 'sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_robot_hw_sim.dir/all' failed
make[1]: *** [sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_robot_hw_sim.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 96%] Building CXX object sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_effort_controller.cpp.o
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See file:///usr/share/doc/gcc-5/README.Bugs for instructions.
sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/build.make:62: recipe for target 'sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/sawyer_gazebo_ros_control_plugin.cpp.o' failed
make[2]: *** [sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/sawyer_gazebo_ros_control_plugin.cpp.o] Error 4
make[2]: *** Waiting for unfinished jobs....
[ 96%] Building CXX object sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_gravity_controller.cpp.o
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See file:///usr/share/doc/gcc-5/README.Bugs for instructions.
sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/build.make:86: recipe for target 'sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/assembly_interface.cpp.o' failed
make[2]: *** [sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/assembly_interface.cpp.o] Error 4
[ 96%] Building CXX object sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/electric_gripper_controller.cpp.o
[ 96%] Building CXX object sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_joint_position_controller.cpp.o
[ 97%] Building CXX object sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_joint_velocity_controller.cpp.o
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See file:///usr/share/doc/gcc-5/README.Bugs for instructions.
sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/build.make:158: recipe for target 'sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/head_interface.cpp.o' failed
make[2]: *** [sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/head_interface.cpp.o] Error 4
[ 97%] Building CXX object sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_joint_effort_controller.cpp.o
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See file:///usr/share/doc/gcc-5/README.Bugs for instructions.
sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/build.make:134: recipe for target 'sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/arm_kinematics_interface.cpp.o' failed
make[2]: *** [sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/src/arm_kinematics_interface.cpp.o] Error 4
CMakeFiles/Makefile2:6819: recipe for target 'sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/all' failed
make[1]: *** [sawyer_simulator/sawyer_gazebo/CMakeFiles/sawyer_gazebo_ros_control.dir/all] Error 2
[ 97%] Building CXX object sawyer_simulator/sawyer_sim_controllers/CMakeFiles/sawyer_sim_controllers.dir/src/sawyer_head_controller.cpp.o
[ 98%] Linking CXX shared library /home/xiaohui/catkin_ws2/devel/lib/libsawyer_sim_controllers.so
[ 98%] Built target sawyer_sim_controllers
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

Sawyer Joint Velocity Issues

When I run the sawer simulator in gazebo (I use ROS Melodic, but the issues also persists for ROS Kinetic), the joint velocity doesn't seem to work. The robot just sags down to the ground. Anyone had the issue, and how can it be fixed?

Finish Setting up Travis-CI