def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to make the adjustment specified by grasp_msg
1. release the hand
2. backup the hand
3. plan a path to the a place 15cm back from the new pose
4. use guarded motion to move forward
"""
print 'adjustment:'
print grasp_msg
#release the object
tp.open_barrett()
#get the robot's current location and calculate the absolute tran after the adjustment
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm)
backup = numpy.matrix([[1,0,0,0],
[0,1,0,0],
[0,0,1,-.05],
[0,0,0,1]]);
back_from_arm_goal = arm_goal * backup
#move back by 10cm
#move_forward(-.1)
#raw_input("Press enter to go to the target pose...")
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
#just go to the new place without a OpenRave trajectory plan
#adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
#blocking motion set to be true
#send_cartesian_relative_goal(self.global_data, adjustInPalm, True)
send_cartesian_goal(back_from_arm_goal, True)
raw_input("Press enter to use guarded motion to move forward...")
#guarded move forward
tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]])
GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
raw_input("Press enter to close the hand...")
#close the hand
#tp.close_barrett()
#tp.move_hand_velocity([0.5,0.5,0.5,0.0])
GuardedCloseHand(self.global_data)
selection = int(raw_input('Lift up (1) or not (0): '))
if selection == 1:
print 'lift up the object'
success = tp.lift_arm(.1, True)
if not success:
grasp_status = graspit_msgs.GraspStatus.UNREACHABLE
grasp_status_msg = "Couldn't lift object"
else:
print 'not lift up the object'
def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to make the adjustment specified by grasp_msg
1. release the hand
2. backup the hand
3. plan a path to the a place 15cm back from the new pose
4. use guarded motion to move forward
"""
print 'adjustment:'
print grasp_msg
#release the object
tp.open_barrett()
#get the robot's current location and calculate the absolute tran after the adjustment
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
arm_goal = tp.convert_cartesian_relative_goal(self.global_data,
adjustInPalm)
backup = numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -.05],
[0, 0, 0, 1]])
back_from_arm_goal = arm_goal * backup
#move back by 10cm
#move_forward(-.1)
#raw_input("Press enter to go to the target pose...")
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
#just go to the new place without a OpenRave trajectory plan
#adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
#blocking motion set to be true
#send_cartesian_relative_goal(self.global_data, adjustInPalm, True)
send_cartesian_goal(back_from_arm_goal, True)
raw_input("Press enter to use guarded motion to move forward...")
#guarded move forward
tp.MoveHandSrv(1, [0, 0, 0, grasp_msg.pre_grasp_dof[0]])
GuardedMoveUntilHit(self.global_data, array([0, 0, 1]), 'PALM', 0.05,
20)
raw_input("Press enter to close the hand...")
#close the hand
#tp.close_barrett()
#tp.move_hand_velocity([0.5,0.5,0.5,0.0])
GuardedCloseHand(self.global_data)
selection = int(raw_input('Lift up (1) or not (0): '))
if selection == 1:
print 'lift up the object'
success = tp.lift_arm(.1, True)
if not success:
grasp_status = graspit_msgs.GraspStatus.UNREACHABLE
grasp_status_msg = "Couldn't lift object"
else:
print 'not lift up the object'
def adjust(self, adjust_msg, close_hand = True):
"""
adjust_msg specifies the adjustment in palm's local coordinate system
"""
print 'adjust:'
print adjust_msg
#release the object
tp.open_barrett()
#tp.move_hand_percentage(0.8)
#get the robot's current location and calculate the absolute tran after the adjustment
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
adjustInPalm = pm.toMatrix(pm.fromMsg(adjust_msg.offset))
arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm)
backup = numpy.matrix([[1,0,0,0],
[0,1,0,0],
[0,0,1,-.05],
[0,0,0,1]]);
back_from_arm_goal = arm_goal * backup
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
#just go to the new place without a OpenRave trajectory plan
#adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
#blocking motion set to be true
#send_cartesian_relative_goal(self.global_data, adjustInPalm, True)
#send_cartesian_goal(back_from_arm_goal, True)
send_cartesian_goal(arm_goal, True)
#raw_input("Press enter to use guarded motion to move forward...")
#guarded move forward
#GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
#raw_input("Press enter to close the hand...")
#close the hand
#tp.close_barrett()
#gentlly close the fingers
if close_hand:
hu.GuardedCloseHand(self.global_data)
tp.move_hand_velocity([0.1,0.1,0.1,0])
def adjust(self, adjust_msg, close_hand=True):
"""
adjust_msg specifies the adjustment in palm's local coordinate system
"""
print 'adjust:'
print adjust_msg
#release the object
tp.open_barrett()
#tp.move_hand_percentage(0.8)
#get the robot's current location and calculate the absolute tran after the adjustment
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
adjustInPalm = pm.toMatrix(pm.fromMsg(adjust_msg.offset))
arm_goal = tp.convert_cartesian_relative_goal(self.global_data,
adjustInPalm)
backup = numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -.05],
[0, 0, 0, 1]])
back_from_arm_goal = arm_goal * backup
#update the robot status
tp.update_robot(self.global_data.or_env.GetRobots()[0])
#just go to the new place without a OpenRave trajectory plan
#adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
#blocking motion set to be true
#send_cartesian_relative_goal(self.global_data, adjustInPalm, True)
#send_cartesian_goal(back_from_arm_goal, True)
send_cartesian_goal(arm_goal, True)
#raw_input("Press enter to use guarded motion to move forward...")
#guarded move forward
#GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
#raw_input("Press enter to close the hand...")
#close the hand
#tp.close_barrett()
#gentlly close the fingers
if close_hand:
hu.GuardedCloseHand(self.global_data)
tp.move_hand_velocity([0.1, 0.1, 0.1, 0])
