# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Sachin Chitta, Jon Binney
import roslib;
roslib.load_manifest('pr2_python')
import rospy
from geometry_msgs.msg import PoseStamped
from pr2_python.arm_mover import ArmMover
# initialize
rospy.init_node('is_in_collision', anonymous=True)
right_arm = ArmMover('right_arm')
if right_arm.is_current_state_in_collision():
print 'Right arm in collision'
else:
print 'Right arm not in collision'
js = right_arm.get_joint_state()
js.position[0] = 1.0 #set shoulder pan (first joint) to swing into left arm
if right_arm.is_in_collision(js):
print 'Right arm in collision'
else:
print 'Right arm not in collision'
