print_arm_states.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('tidyup_tools')
00004 import rospy
00005 import sys
00006 import copy
00007 import numpy as np
00008 from arm_navigation_msgs.msg import RobotState, PlanningScene
00009 from geometry_msgs.msg import Point, Pose, PoseStamped
00010 from tidyup_arm_services.planning_scene_sync import PlanningSceneSync
00011 from pr2_python.planning_scene_interface import get_planning_scene_interface
00012 
00013 if __name__ == '__main__':
00014   rospy.init_node('print_arm_angles')
00015   psi = get_planning_scene_interface()
00016   pss = PlanningSceneSync()
00017   pss._latest_planning_scene.robot_state = psi.get_robot_state()
00018 
00019   left_arm_joints = [2.1, 1.26, 1.8, -1.9, -3.5, -1.8, np.pi/2.0]
00020   right_arm_joints = [-2.1, 1.26, -1.8, -1.9, 3.5, -1.8, np.pi/2.0]
00021   loop_rate = rospy.Rate(0.2)
00022 #  rospy.loginfo('checking current robot state for collisions...')
00023   while not rospy.is_shutdown():
00024     robot_state = pss._latest_planning_scene.robot_state
00025     r = robot_state.joint_state.name.index('r_shoulder_pan_joint')
00026     l = robot_state.joint_state.name.index('l_shoulder_pan_joint')
00027     for offset in range(len(right_arm_joints)):
00028       right_arm_joints[offset] = robot_state.joint_state.position[r + offset]
00029       left_arm_joints[offset] = robot_state.joint_state.position[l + offset]
00030     rospy.loginfo('right_arm: \n{0}'.format(right_arm_joints))
00031     rospy.loginfo('left_arm: \n{0}'.format(left_arm_joints))
00032     loop_rate.sleep()
00033 
00034   
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


tidyup_tools
Author(s): Andreas Hertle, Christian Dornhege
autogenerated on Wed Dec 26 2012 15:50:57