def __init__(self, whicharm, is_sim):
self.is_sim = is_sim
self.gen_utils = generalUtils.GeneralUtils()
self.traj_utils = trajUtils.TrajUtils()
#Set up right/left arm variables
if (whicharm == 0):
self.gripper_topic_name = '/r_gripper_controller/state'
fk_serv_name = '/pr2_right_arm_kinematics/get_fk'
fk_joints = [
"r_shoulder_pan_joint", "r_shoulder_lift_joint",
"r_upper_arm_roll_joint", "r_elbow_flex_joint",
"r_forearm_roll_joint", "r_wrist_flex_joint",
"r_wrist_roll_joint"
]
self.link_name = 'r_wrist_roll_link'
self.mann_pos_topic_name = '/r_arm_controller_loose/state'
self.cont_pos_topic_name = '/r_arm_controller/state'
else:
self.gripper_topic_name = '/l_gripper_controller/state'
fk_serv_name = '/pr2_left_arm_kinematics/get_fk'
fk_joints = [
"l_shoulder_pan_joint", "l_shoulder_lift_joint",
"l_upper_arm_roll_joint", "l_elbow_flex_joint",
"l_forearm_roll_joint", "l_wrist_flex_joint",
"l_wrist_roll_joint"
]
self.link_name = 'l_wrist_roll_link'
self.mann_pos_topic_name = '/l_arm_controller_loose/state'
self.cont_pos_topic_name = '/l_arm_controller/state'
#Fill this in later, once we figure out what we are currently looking at
self.pos_topic_name = ''
#There must be a planning scene or FK / IK crashes
#print 'Waiting for set planning scene service...'
#rospy.wait_for_service('/environment_server/set_planning_scene_diff')
#setPlan = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', arm_navigation_msgs.srv.SetPlanningSceneDiff)
#req = arm_navigation_msgs.srv.SetPlanningSceneDiffRequest()
#setPlan(req)
#print 'OK'
print 'Waiting for forward kinematics service...'
rospy.wait_for_service(fk_serv_name)
self.getPosFK = rospy.ServiceProxy(fk_serv_name,
kinematics_msgs.srv.GetPositionFK,
persistent=True)
print "OK"
self.FKreq = kinematics_msgs.srv.GetPositionFKRequest()
self.FKreq.header.frame_id = "torso_lift_link"
self.FKreq.fk_link_names = [self.link_name]
self.FKreq.robot_state.joint_state.name = fk_joints
def __init__(self):
self.tformer = tf.TransformerROS(True, rospy.Duration(10.0))
self.wm = arWorldModel.ARWorldModel()
self.gen_utils = generalUtils.GeneralUtils()
def __init__(self):
self.rviz_pub = rospy.Publisher('dmp_goal_arrow', vm.Marker)
self.rviz_pub2 = rospy.Publisher('grip_pos_arrow', vm.Marker)
self.tf_broad = tf.TransformBroadcaster()
self.gen_utils = generalUtils.GeneralUtils()
if diff > goal_thresh[i]:
conv = False
return conv
if __name__ == '__main__':
try:
rospy.init_node('SingleReplayNode')
#rospy.on_shutdown(shutdown_cb)
RIGHT_ARM = 0
LEFT_ARM = 1
#Setup utilities and world model
gen_utils = generalUtils.GeneralUtils()
traj_utils = trajUtils.TrajUtils()
move_utils = moveUtils.MoveUtils()
draw_utils = drawUtils.DrawUtils()
wm = arWorldModel.ARWorldModel()
#Parameters that select a skill to execute and the marker frame it is in (-1 for torso)
if (len(sys.argv) == 6):
whicharm = int(sys.argv[1])
skill_id = int(sys.argv[2])
control_frame = int(sys.argv[3])
goal_frame = int(sys.argv[4])
plan_only = int(sys.argv[5])
else:
print "\nAborting! Wrong number of command line args"
print "Usage: python singleReplay <whicharm> <skill_id> <control_frame> <goal_frame> <plan_only>"
def __init__(self):
self.gen_utils = generalUtils.GeneralUtils()
self.traj_utils = trajUtils.TrajUtils()
self.move_utils = moveUtils.MoveUtils()
self.draw_utils = drawUtils.DrawUtils()
self.wm = arWorldModel.ARWorldModel()
def __init__(self):
self.gen_utils = generalUtils.GeneralUtils()
self.move_utils = moveUtils.MoveUtils()
self.obj_lock = threading.Lock()
self.objects = dict()
self.pose_locks = []
self.pose_locks.append(threading.Lock())
self.pose_locks.append(threading.Lock())
self.arm_cart_poses = [[] for i in xrange(2)]
#There must be a planning scene or FK / IK crashes
print 'Waiting for set planning scene service...'
rospy.wait_for_service('/environment_server/set_planning_scene_diff')
setPlan = rospy.ServiceProxy(
'/environment_server/set_planning_scene_diff',
SetPlanningSceneDiff)
req = SetPlanningSceneDiffRequest()
setPlan(req)
print 'OK'
#Set up right/left FK service connections
print 'Waiting for forward kinematics services...'
rospy.wait_for_service('/pr2_right_arm_kinematics/get_fk')
self.getPosFK = []
self.getPosFK.append(
rospy.ServiceProxy('/pr2_right_arm_kinematics/get_fk',
GetPositionFK,
persistent=True))
self.getPosFK.append(
rospy.ServiceProxy('/pr2_left_arm_kinematics/get_fk',
GetPositionFK,
persistent=True))
print "OK"
#Set up right/left FK service requests
self.FKreq = [
kinematics_msgs.srv.GetPositionFKRequest(),
kinematics_msgs.srv.GetPositionFKRequest()
]
self.FKreq[0].header.frame_id = "torso_lift_link"
self.FKreq[0].fk_link_names = ['r_wrist_roll_link']
self.FKreq[0].robot_state.joint_state.name = [
"r_shoulder_pan_joint", "r_shoulder_lift_joint",
"r_upper_arm_roll_joint", "r_elbow_flex_joint",
"r_forearm_roll_joint", "r_wrist_flex_joint", "r_wrist_roll_joint"
]
self.FKreq[1].header.frame_id = "torso_lift_link"
self.FKreq[1].fk_link_names = ['l_wrist_roll_link']
self.FKreq[1].robot_state.joint_state.name = [
"l_shoulder_pan_joint", "l_shoulder_lift_joint",
"l_upper_arm_roll_joint", "l_elbow_flex_joint",
"l_forearm_roll_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"
]
#Set up right/left pose topic subscriptions
rospy.Subscriber('/r_arm_controller/state',
JointTrajectoryControllerState, self.rArmPosCallback)
rospy.Subscriber('/l_arm_controller/state',
JointTrajectoryControllerState, self.lArmPosCallback)
#Set up AR tag topic subscription
rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.arPoseMarkerCallback)
#Set up wm publisher
self.wm_pub = rospy.Publisher('/ar_world_model',
pr2_lfd_utils.msg.WMData)
self.rmp_lock = threading.Lock()
self.rmp_total_weight = 0.0
self.rmp_pose = geometry_msgs.msg.PoseStamped()
self.rmp_which_marker = -1
self.rmp_reset = False