def close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
rospy.sleep(1)
grasp_status = self.get_grasp_status_srv()
return grasp_status.is_hand_occupied
def close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
rospy.sleep(1)
grasp_status = self.get_grasp_status_srv()
return grasp_status.is_hand_occupied
def grasp_exec(self, grasp, timeout_sec=15.0):
hand_posture_goal_ = GraspHandPostureExecutionGoal()
hand_posture_goal_.grasp = grasp
# do grasp action
hand_posture_goal_.goal = GraspHandPostureExecutionGoal.GRASP
hand_posture_goal_.max_contact_force = 1.0
# call hand_posture_exec action lib
rospy.loginfo("Trying to grasp...")
return self.hand_posture_exec(hand_posture_goal_,timeout_sec)
def grasp_release_exec(self,timeout_sec=10.0):
hand_posture_goal_ = GraspHandPostureExecutionGoal()
hand_posture_goal_.grasp = Grasp()
hand_posture_goal_.grasp.pre_grasp_posture.name= [ "FFJ0", "FFJ3", "FFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3", "RFJ4", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"]
hand_posture_goal_.grasp.pre_grasp_posture.position = [0]*(18)
# do release action
hand_posture_goal_.goal = GraspHandPostureExecutionGoal.RELEASE
# call hand_posture_exec action lib
rospy.loginfo("Trying to release...")
return self.hand_posture_exec(hand_posture_goal_,timeout_sec)
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
result = self.posture_controller.send_goal_and_wait(pg)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to open gripper')
return False
return True
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
result = self.posture_controller.send_goal_and_wait(pg)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to open gripper')
return False
return True
def gentle_close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.CLOSE_GRIPPER
goal.torque_limit = 0.0
goal.dynamic_torque_control = False
self.gripper_controller.send_goal(goal)
self.gripper_controller.wait_for_result()
def gentle_close_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
goal = WubbleGripperGoal()
goal.command = WubbleGripperGoal.CLOSE_GRIPPER
goal.torque_limit = 0.0
goal.dynamic_torque_control = False
self.gripper_controller.send_goal(goal)
self.gripper_controller.wait_for_result()
def grasp_connector(self, grasp):
#First we set the hand to the pregrasp position
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
self.grasp_client.send_goal( goal )
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Pregrasp")
return False
self.go_to_middle_2_position()
#then we move the arm to the grasp pose
goal = TrajectoryGoal()
traj = []
speed = []
traj.append( grasp.grasp_pose )
speed.append( 45. )
goal.trajectory = traj
goal.speed = speed
self.denso_trajectory_client.send_goal( goal )
self.denso_trajectory_client.wait_for_result()
res = self.denso_trajectory_client.get_result()
if res.val != TrajectoryResult.SUCCESS:
rospy.logerr("Failed to move the arm to the given position.")
return False
#finally we grasp the object
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
rospy.loginfo("Going to grasp")
goal.goal = goal.GRASP
self.grasp_client.send_goal( goal )
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Grasp")
return False
rospy.loginfo("OK grasped")
return True
def unplug_connector(self, grasp):
#We compute a list of poses to send to
# the arm (going up from grasp)
rospy.loginfo("unplugging the connector")
goal = TrajectoryGoal()
traj = []
speed = []
pose_tmp = grasp.grasp_pose
pose_tmp.position.z += 0.1
traj.append(pose_tmp)
speed.append(30.)
goal.trajectory = traj
goal.speed = speed
self.denso_trajectory_client.send_goal(goal)
self.denso_trajectory_client.wait_for_result()
res = self.denso_trajectory_client.get_result()
if res.val != TrajectoryResult.SUCCESS:
rospy.logerr("Failed to move the arm to the given position.")
return False
rospy.loginfo("unplugged the connector")
#Move the arm to a fixed position (above the box where the connector will be dropped)
goal = TrajectoryGoal()
traj = []
speed = []
pose_above_box = Pose()
pose_above_box.position.x = 0.454051
pose_above_box.position.y = 0.283831
pose_above_box.position.z = 0.296853
pose_above_box.orientation.x = 0.711336
pose_above_box.orientation.y = -0.31226
pose_above_box.orientation.z = 0.577
pose_above_box.orientation.w = 0.25212
traj.append(pose_above_box)
speed.append(100.)
#pose_in_box = Pose()
#pose_in_box.position.x = 0.561363
#pose_in_box.position.y = 0.229195
#pose_in_box.position.z = 0.228286
#pose_in_box.orientation.x = 0.772113
#pose_in_box.orientation.y = -0.318396
#pose_in_box.orientation.z = 0.532802
#pose_in_box.orientation.w = 0.198744
#traj.append( pose_in_box )
#speed.append( 10. )
goal.trajectory = traj
goal.speed = speed
rospy.loginfo("Putting the connector in the box")
self.denso_trajectory_client.send_goal(goal)
self.denso_trajectory_client.wait_for_result()
res = self.denso_trajectory_client.get_result()
if res.val != TrajectoryResult.SUCCESS:
rospy.logerr("Failed to move the arm to the given position.")
return False
#finally we release the object
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
rospy.loginfo("Going to release the object")
goal.goal = goal.RELEASE
self.grasp_client.send_goal(goal)
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Release")
return False
time.sleep(0.1)
rospy.loginfo("Released the connector")
return True
def grasp_connector(self, grasp):
#First we set the hand to the pregrasp position
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
self.grasp_client.send_goal(goal)
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Pregrasp")
return False
self.go_to_middle_2_position()
#then we move the arm to the grasp pose
goal = TrajectoryGoal()
traj = []
speed = []
traj.append(grasp.grasp_pose)
speed.append(45.)
goal.trajectory = traj
goal.speed = speed
self.denso_trajectory_client.send_goal(goal)
self.denso_trajectory_client.wait_for_result()
res = self.denso_trajectory_client.get_result()
if res.val != TrajectoryResult.SUCCESS:
rospy.logerr("Failed to move the arm to the given position.")
return False
#finally we grasp the object
goal = GraspHandPostureExecutionGoal()
goal.grasp = grasp
goal.goal = goal.PRE_GRASP
rospy.loginfo("Going to grasp")
goal.goal = goal.GRASP
self.grasp_client.send_goal(goal)
self.grasp_client.wait_for_result()
res = self.grasp_client.get_result()
if res.result.value != ManipulationResult.SUCCESS:
rospy.logerr("Failed to go to Grasp")
return False
rospy.loginfo("OK grasped")
return True
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
def open_gripper(self):
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
self.posture_controller.send_goal(pg)
self.posture_controller.wait_for_result()
import roslib; roslib.load_manifest('wubble2_robot')
import rospy
from actionlib import SimpleActionClient
from object_manipulation_msgs.msg import GraspHandPostureExecutionAction
from object_manipulation_msgs.msg import GraspHandPostureExecutionGoal
if __name__ == '__main__':
rospy.init_node('test_gripper_posture_controller', anonymous=True)
posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
posture_controller.wait_for_server()
rospy.loginfo('got gripper_posture_controller')
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
posture_controller.send_goal(pg)
posture_controller.wait_for_result()
rospy.loginfo('finished releasing')
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
posture_controller.send_goal(pg)
posture_controller.wait_for_result()
rospy.loginfo('finished grasping')
