def go_to_middle_2_position(self): #Move the arm to a fixed middle position goal = TrajectoryGoal() traj = [] speed = [] pose_tmp = Pose() pose_tmp.position.x = 0.217902 pose_tmp.position.y = 0.213502 pose_tmp.position.z = 0.339546 pose_tmp.orientation.x = 0.700753 pose_tmp.orientation.y = -0.504614 pose_tmp.orientation.z = 0.702508 pose_tmp.orientation.w = 0.0511 traj.append(pose_tmp) speed.append(100.) 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 return True
def go_to_final_position(self): #Move the arm to a fixed final position goal = TrajectoryGoal() traj = [] speed = [] pose_tmp = Pose() pose_tmp.position.x = 0.138631 pose_tmp.position.y = 0.433044 pose_tmp.position.z = 0.44185 pose_tmp.orientation.x = 0.700738 pose_tmp.orientation.y = -0.254597 pose_tmp.orientation.z = 0.652538 pose_tmp.orientation.w = 0.135428 traj.append(pose_tmp) speed.append(100.) 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 return True
def go_to_final_position(self): #Move the arm to a fixed final position goal = TrajectoryGoal() traj = [] speed = [] pose_tmp = Pose() pose_tmp.position.x = 0.138631 pose_tmp.position.y = 0.433044 pose_tmp.position.z = 0.44185 pose_tmp.orientation.x = 0.700738 pose_tmp.orientation.y = -0.254597 pose_tmp.orientation.z = 0.652538 pose_tmp.orientation.w = 0.135428 traj.append( pose_tmp ) speed.append( 100. ) 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 return True
def go_to_initial_position(self): #Move the arm to a fixed initial position goal = TrajectoryGoal() traj = [] speed = [] pose_tmp = Pose() pose_tmp.position.x = 0.157454 pose_tmp.position.y = 0.536324 pose_tmp.position.z = 0.357698 pose_tmp.orientation.x = 0.736802 pose_tmp.orientation.y = 0.147973 pose_tmp.orientation.z = 0.500246 pose_tmp.orientation.w = 0.430094 traj.append(pose_tmp) speed.append(100.) 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 return True
def go_to_middle_2_position(self): #Move the arm to a fixed middle position goal = TrajectoryGoal() traj = [] speed = [] pose_tmp = Pose() pose_tmp.position.x = 0.217902 pose_tmp.position.y = 0.213502 pose_tmp.position.z = 0.339546 pose_tmp.orientation.x = 0.700753 pose_tmp.orientation.y = -0.504614 pose_tmp.orientation.z = 0.702508 pose_tmp.orientation.w = 0.0511 traj.append( pose_tmp ) speed.append( 100. ) 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 return True
def go_to_initial_position(self): #Move the arm to a fixed initial position goal = TrajectoryGoal() traj = [] speed = [] pose_tmp = Pose() pose_tmp.position.x = 0.157454 pose_tmp.position.y = 0.536324 pose_tmp.position.z = 0.357698 pose_tmp.orientation.x = 0.736802 pose_tmp.orientation.y = 0.147973 pose_tmp.orientation.z = 0.500246 pose_tmp.orientation.w = 0.430094 traj.append( pose_tmp ) speed.append( 100. ) 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 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 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 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