def cartesian_planning(self, cmd):
if (cmd[0] == 'PICK'):
Robotiq.open(self.gripper, block=True)
rospy.sleep(1)
waypoint = []
'''
wpose is initialized twice to prevent inconsistent pose value
which may happen when get_current_pose() is called first.
'''
wpose = self.ur5.get_current_pose().pose
wpose = self.ur5.get_current_pose().pose
waypoint.append(copy.deepcopy(wpose))
# Move TCP to target position
wpose.position.y += round(float(cmd[2]), 3)
wpose.position.z += round(float(cmd[3]), 3) + 0.03
wpose.position.x += round(float(cmd[1]), 3)
waypoint.append(copy.deepcopy(wpose))
wpose.position.z -= 0.03
waypoint.append(copy.deepcopy(wpose))
plan, fraction = self.ur5.compute_cartesian_path(waypoint, 0.01, 0.0)
print('Fraction : {}'.format(fraction))
self.ur5.execute(plan, wait=True)
rospy.sleep(1)
# Pick or Place or TCP Move Only
if (cmd[0] == 'PICK'):
Robotiq.close(self.gripper, block=True)
elif (cmd[0] == 'PLACE'):
Robotiq.open(self.gripper, block=True)
else:
return
rospy.sleep(1)
# Planning backward to the base pose
rev_plan = self.reverse_planning(plan)
self.ur5.execute(rev_plan, wait=True)
self.ur5.stop()
self.ur5.clear_pose_targets()
print('cartesian planning done')
return
pose_goal.position.x = current_pose.position.x
pose_goal.position.y = current_pose.position.y
pose_goal.position.z = current_pose.position.z - 0.1
group.set_pose_target(pose_goal)
plan = group.plan()
group.go(wait=True)
if key == 'c':
print "========close gripper======="
Robotiq.close(robotiq_client, speed=0.1, force=0, block=True)
if key == 'o':
print "========close gripper======="
Robotiq.open(robotiq_client, block=True)
'''current_pose=group.get_current_pose().pose
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.x = current_pose.orientation.x
pose_goal.orientation.y = current_pose.orientation.y
pose_goal.orientation.z = current_pose.orientation.z
pose_goal.orientation.w = current_pose.orientation.w
pose_goal.position.x = current_pose.position.x
pose_goal.position.y = current_pose.position.y
pose_goal.position.z = current_pose.position.z-0.1
group.set_pose_target(pose_goal)
plan = group.plan()
group.go(wait=True)
