def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to grasp the object and lift it
First moves the arm to pregrasp the object, then attempts to grasp and lift it.
The grasp and lifting phase is currently completely open loop
"""
try:
#Lock the openrave environment - it will restore the robot and collision state
#to it's previous state when it finishes automagically.
with self.global_data.or_env:
#Reject multiple grasp messages sent too fast. They are likely a mistake
#FIXME - We should probably just set the queue size to 1.
if (time() - self.last_grasp_time) < 30:
return [], []
self.last_grasp_time = time()
print grasp_msg
grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS
grasp_status_msg = "grasp_succeeded"
success = 1
# Send the robot to its home position if it is actually running
#and not currently there
if self.global_data.robot_running and not tp.is_home():
print 'go home'
tp.go_home(self.global_data)
#This sometimes fails near the end of the trajectory because the last few
#steps of the trajectory are slow because our blending parameters
#are probably wrong. It is basically in the home position, so we
#don't check for success in reaching the home position because meaningful
#failures are rare.
#Open the hand - Leaves spread angle unchanged
if success and self.global_data.robot_running:
success, grasp_status_msg, positions = tp.open_barrett()
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
if success:
#Pregrasp the object
#Convert the grasp message to a openrave transform
grasp_tran = pm.toMatrix(
pm.fromMsg(grasp_msg.final_grasp_pose))
grasp_tran[0:3, 3] /= 1000 #mm to meters
#Move the hand to the pregrasp spread angle - Update openrave's robot pose
if self.global_data.robot_running:
tp.MoveHandSrv(1,
[0, 0, 0, grasp_msg.pre_grasp_dof[0]])
tp.update_robot(self.global_data.or_env.GetRobots()[0])
print 'pre-grasp'
#Publish the object TFs
self.model_manager()
print 'after model_manager()'
#Calculate a trajectory to the pregrasp pose and move to it if necessary
success, final_tran, dof_list, j = tp.pregrasp_object(
self.global_data,
grasp_msg.object_name,
False,
grasp_tran,
run_it=self.global_data.robot_running)
print 'after pre-grasp'
#Failures shouldn't happen if grasp analysis is being used, so that's wierd.
if not success:
pdb.set_trace()
#Make sure that the openrave environment is up to date in pregrasp pose
if self.global_data.robot_running:
tp.update_robot(self.global_data.or_env.GetRobots()[0])
else:
#If the robot isn't actually running, just update the openrave visualization
#so that the user can see what was planned.
if success:
print "setting robot joints"
print j
self.global_data.or_env.GetRobots(
)[0].SetActiveDOFs(range(6))
self.global_data.or_env.GetRobots(
)[0].SetActiveDOFValues(dof_list[-1])
self.global_data.or_env.GetRobots(
)[0].SetActiveDOFs(range(6, 10))
self.global_data.or_env.GetRobots(
)[0].SetActiveDOFValues(
list(grasp_msg.pre_grasp_dof[1:]) +
[grasp_msg.pre_grasp_dof[0]])
self.global_data.or_env.GetRobots(
)[0].SetActiveDOFs(range(6))
self.global_data.or_env.UpdatePublishedBodies()
pdb.set_trace()
success = False
#If the pregrasp was unreachable, record the type of issue
if not success:
#If there is no joint list, the pregrasp transform had no inverse
#kinematics solution, probably.
if not j:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Pregrasp tran Out of range!"
#Otherwise, something even wierder happened.
else:
grasp_status = graspit_msgs.msg.GraspStatus.FAILED
grasp_status_msg = "Unknown planner failure!"
#Move from the pregrasp to the final grasp pose in a straight line along the approach
#direction
if success:
if not Hao:
#Move forward open loop
success = tp.move_forward(0.05, True)
else:
#Move forward using force transducer and palm contact sensors to detect any
#collisions
hu.GuardedMoveUntilHit(self.global_data, array([0, 0, 1]),
'PALM', 0.05, 20)
success = True
if not success:
#If we failed to move forward - which might happen because
#we do not test if the final grasp pose is reachable along
#a straight line trajectory because the joint range limits
#set on the robot are not actually exactly the same as those
#in openrave and it usually doesn't cause any problems
#in general trajectory testing.
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Unable to move forward to grasp!"
if Hao:
"""new routine"""
#Close the hand using the force torque and contact sensors to detect contacts and
#stop the fingers
hu.GuardedCloseHand(self.global_data)
else:
"""old routine"""
if success:
#Just close the hand open loop, leaving the spread angle where it is now.
success, grasp_status_msg, joint_angles = tp.move_hand([
grasp_msg.pre_grasp_dof[1], grasp_msg.pre_grasp_dof[2],
grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]
])
if success:
#Close the hand completely until the motors stall or they hit
#the final grasp DOFS
success, grasp_status_msg, joint_angles = tp.move_hand([
grasp_msg.final_grasp_dof[1],
grasp_msg.final_grasp_dof[2],
grasp_msg.final_grasp_dof[3],
grasp_msg.final_grasp_dof[0]
])
if success:
#Now close the hand completely until the motors stall.
success, grasp_status_msg, joint_angles = tp.close_barrett(
)
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
#Now wait for user input on whether or not to lift the object
if success:
selection = int(raw_input('Lift up (1) or not (0): '))
if selection == 1:
print 'lift up the object'
#Lift the object using the staubli's straight line path planner
success = tp.lift_arm(.05, True)
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Couldn't lift object"
else:
print 'not lift up the object'
#Maybe decide if it has been successfully lifted here...
if success:
rospy.logwarn(grasp_status_msg)
else:
rospy.logfatal(grasp_status_msg)
#Tell graspit whether the grasp succeeded
self.graspit_status_publisher.publish(grasp_status,
grasp_status_msg, -1)
print grasp_status_msg
return grasp_status, grasp_status_msg
except Exception as e:
#print any exceptions and drop in to the debugger.
import traceback
print traceback.format_exc()
pdb.set_trace()
def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to grasp the object and lift it
First moves the arm to pregrasp the object, then attempts to grasp and lift it.
The grasp and lifting phase is currently completely open loop
"""
try:
#Lock the openrave environment - it will restore the robot and collision state
#to it's previous state when it finishes automagically.
with self.global_data.or_env:
#Reject multiple grasp messages sent too fast. They are likely a mistake
#FIXME - We should probably just set the queue size to 1.
if (time() - self.last_grasp_time) < 30:
return [], []
self.last_grasp_time = time()
print grasp_msg
grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS
grasp_status_msg = "grasp_succeeded"
success = 1
# Send the robot to its home position if it is actually running
#and not currently there
if self.global_data.robot_running and not tp.is_home():
print 'go home'
tp.go_home(self.global_data)
#This sometimes fails near the end of the trajectory because the last few
#steps of the trajectory are slow because our blending parameters
#are probably wrong. It is basically in the home position, so we
#don't check for success in reaching the home position because meaningful
#failures are rare.
#Open the hand - Leaves spread angle unchanged
if success and self.global_data.robot_running:
success, grasp_status_msg, positions = tp.open_barrett()
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
if success:
#Pregrasp the object
#Convert the grasp message to a openrave transform
grasp_tran = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
grasp_tran[0:3,3] /=1000 #mm to meters
#Move the hand to the pregrasp spread angle - Update openrave's robot pose
if self.global_data.robot_running:
tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]])
tp.update_robot(self.global_data.or_env.GetRobots()[0])
print 'pre-grasp'
#Publish the object TFs
self.model_manager()
print 'after model_manager()'
#Calculate a trajectory to the pregrasp pose and move to it if necessary
success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, grasp_msg.object_name, False, grasp_tran, run_it = self.global_data.robot_running)
print 'after pre-grasp'
#Failures shouldn't happen if grasp analysis is being used, so that's wierd.
if not success:
pdb.set_trace()
#Make sure that the openrave environment is up to date in pregrasp pose
if self.global_data.robot_running:
tp.update_robot(self.global_data.or_env.GetRobots()[0])
else:
#If the robot isn't actually running, just update the openrave visualization
#so that the user can see what was planned.
if success:
print "setting robot joints"
print j
self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6))
self.global_data.or_env.GetRobots()[0].SetActiveDOFValues(dof_list[-1])
self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6,10))
self.global_data.or_env.GetRobots()[0].SetActiveDOFValues(list(grasp_msg.pre_grasp_dof[1:]) + [grasp_msg.pre_grasp_dof[0]])
self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6))
self.global_data.or_env.UpdatePublishedBodies()
pdb.set_trace()
success = False
#If the pregrasp was unreachable, record the type of issue
if not success:
#If there is no joint list, the pregrasp transform had no inverse
#kinematics solution, probably.
if not j:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Pregrasp tran Out of range!"
#Otherwise, something even wierder happened.
else:
grasp_status = graspit_msgs.msg.GraspStatus.FAILED
grasp_status_msg = "Unknown planner failure!"
#Move from the pregrasp to the final grasp pose in a straight line along the approach
#direction
if success:
if not Hao:
#Move forward open loop
success = tp.move_forward(0.05, True)
else:
#Move forward using force transducer and palm contact sensors to detect any
#collisions
hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
success = True
if not success:
#If we failed to move forward - which might happen because
#we do not test if the final grasp pose is reachable along
#a straight line trajectory because the joint range limits
#set on the robot are not actually exactly the same as those
#in openrave and it usually doesn't cause any problems
#in general trajectory testing.
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Unable to move forward to grasp!"
if Hao:
"""new routine"""
#Close the hand using the force torque and contact sensors to detect contacts and
#stop the fingers
hu.GuardedCloseHand(self.global_data)
else:
"""old routine"""
if success:
#Just close the hand open loop, leaving the spread angle where it is now.
success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.pre_grasp_dof[1],grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]])
if success:
#Close the hand completely until the motors stall or they hit
#the final grasp DOFS
success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.final_grasp_dof[1],grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0]])
if success:
#Now close the hand completely until the motors stall.
success, grasp_status_msg, joint_angles = tp.close_barrett()
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
#Now wait for user input on whether or not to lift the object
if success:
selection = int(raw_input('Lift up (1) or not (0): '))
if selection == 1:
print 'lift up the object'
#Lift the object using the staubli's straight line path planner
success = tp.lift_arm(.05, True)
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Couldn't lift object"
else:
print 'not lift up the object'
#Maybe decide if it has been successfully lifted here...
if success:
rospy.logwarn(grasp_status_msg)
else:
rospy.logfatal(grasp_status_msg)
#Tell graspit whether the grasp succeeded
self.graspit_status_publisher.publish(grasp_status, grasp_status_msg, -1)
print grasp_status_msg
return grasp_status, grasp_status_msg
except Exception as e:
#print any exceptions and drop in to the debugger.
import traceback
print traceback.format_exc()
pdb.set_trace()
def process_grasp_msg(self, grasp_msg):
"""@brief - Attempt to grasp the object and lift it
First moves the arm to pregrasp the object, then attempts to grasp and lift it.
The grasp and lifting phase is currently completely open loop
"""
if (time() - self.last_grasp_time) < 30:
return [], []
self.last_grasp_time = time()
print grasp_msg
grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS
grasp_status_msg = "grasp_succeeded"
success = 1
if not tp.is_home():
print 'go home'
tp.go_home(self.global_data)
# if not success:
# grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
# grasp_status_msg = "Unable to go home"
if success:
success, grasp_status_msg, positions = tp.open_barrett()
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
if success:
grasp_tran = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
grasp_tran[0:3,3] /=1000 #mm to meters
tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]])
tp.update_robot(self.global_data.or_env.GetRobots()[0])
print 'pre-grasp'
self.model_manager()
# success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, file_name_dict[self.target_object_name], grasp_tran)
print 'after model_manager()'
success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, self.target_object_name, False, grasp_tran)
print 'after pre-grasp'
#raw_input("Press enter...")
tp.update_robot(self.global_data.or_env.GetRobots()[0])
if not success:
if not j:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Pregrasp tran Out of range!"
else:
grasp_status = graspit_msgs.msg.GraspStatus.FAILED
grasp_status_msg = "Unknown planner failure!"
if success:
if not Hao:
success = tp.move_forward(0.05, True)
else:
hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
success = True
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Unable to move forward to grasp!"
if Hao:
"""new routine"""
hu.GuardedCloseHand(self.global_data)
else:
"""old routine"""
if success:
success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.pre_grasp_dof[1],grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]])
if success:
success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.final_grasp_dof[1],grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0]])
if success:
success, grasp_status_msg, joint_angles = tp.close_barrett()
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
if success:
selection = int(raw_input('Lift up (1) or not (0): '))
if selection == 1:
print 'lift up the object'
success = tp.lift_arm(.05, True)
if not success:
grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
grasp_status_msg = "Couldn't lift object"
else:
print 'not lift up the object'
#Maybe decide if it has been successfully lifted here...
if success:
rospy.logwarn(grasp_status_msg)
else:
rospy.logfatal(grasp_status_msg)
self.graspit_status_publisher.publish(grasp_status, grasp_status_msg)
print grasp_status_msg
return grasp_status, grasp_status_msg