def _moveRightArmToStart():
pa = PhysicalAgent('right_gripper')
starting_joint_angles_r = {
'right_e0': -0.39888044530362166,
'right_e1': 1.9341522973651006,
'right_s0': 0.936293285623961,
'right_s1': -0.9939970420424453,
'right_w0': 0.27417171168213983,
'right_w1': 0.8298780975195674,
'right_w2': -0.5085333554167599
}
pa.move_to_start(starting_joint_angles_r)
def _moveLeftArmToStart():
pa = PhysicalAgent('left_gripper')
starting_joint_angles_l = {
'left_w0': 0.6699952259595108,
'left_w1': 1.030009435085784,
'left_w2': -0.4999997247485215,
'left_e0': -1.189968899785275,
'left_e1': 1.9400238130755056,
'left_s0': -0.08000397926829805,
'left_s1': -0.9999781166910306
}
pa.move_to_start(starting_joint_angles_l)
def main():
rospy.init_node("partial_plan_executor_node")
rospy.wait_for_message("/robot/sim/started", Empty)
global lPA
global rPA
lPA = PhysicalAgent(limb='left_gripper', hover_distance=0.0) #
rPA = PhysicalAgent(limb='right_gripper', hover_distance=0.0) #
s = rospy.Service("partial_plan_executor_srv", PartialPlanExecutorSrv,
execute_action) #rewrite
rospy.spin()
return 0
def main():
rospy.init_node("physical_agent_node")
rospy.wait_for_message("/robot/sim/started", Empty)
global pa
pa = PhysicalAgent()
s_1 = rospy.Service("move_to_start_srv", MoveToStartSrv, move_to_start)
s_2 = rospy.Service("toggle_gripper_state_srv", ToggleGripperStateSrv,
toggle_gripper_state)
s_3 = rospy.Service("approach_srv", ApproachSrv, approach)
rospy.spin()
return 0
def main():
rospy.init_node("physical_agent_node")
global pa
pa = PhysicalAgent()
s_1 = rospy.Service("move_to_start_srv", MoveToStartSrv, move_to_start)
s_2 = rospy.Service("open_gripper_srv", OpenGripperSrv, open_gripper)
s_2 = rospy.Service("close_gripper_srv", CloseGripperSrv, close_gripper)
s_3 = rospy.Service("approach_srv", ApproachSrv, approach)
rospy.spin()
return 0
def handle_ObtainObject(req):
global limb
limb = req.limb
global object_name
object_name = req.objectName
poseTo = None
if object_name == "left_button":
poseTo = LeftButtonPose
elif object_name == "right_button":
poseTo = RightButtonPose
else:
poseTo = BlockPose
hover_distance = 0.15
# Shouldnt have to start at starting pose
if limb == 'left_gripper':
starting_joint_angles_l = {'left_w0': 0.6699952259595108,
'left_w1': 1.030009435085784,
'left_w2': -0.4999997247485215,
'left_e0': -1.189968899785275,
'left_e1': 1.9400238130755056,
'left_s0': -0.08000397926829805,
'left_s1': -0.9999781166910306}
else:
starting_joint_angles_r = {'right_e0': -0.39888044530362166,
'right_e1': 1.9341522973651006,
'right_s0': 0.936293285623961,
'right_s1': -0.9939970420424453,
'right_w0': 0.27417171168213983,
'right_w1': 0.8298780975195674,
'right_w2': -0.5085333554167599}
currentAction = PhysicalAgent(limb, hover_distance)
if limb == 'left_gripper':
currentAction.move_to_start(starting_joint_angles_l)
else:
currentAction.move_to_start(starting_joint_angles_r)
currentAction.gripper_open()
currentAction.approach(hoverOverPose(poseTo))
currentAction.approach(grabPose(poseTo))
currentAction.gripper_close()
currentAction.approach(hoverOverPose(poseTo))
if limb == 'left_gripper':
currentAction.move_to_start(starting_joint_angles_l)
else:
currentAction.move_to_start(starting_joint_angles_r)
return ObtainObjectSrvResponse(1)