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