use cartesian plaln and display and confirm (#18)

calibrations/testfile.yaml

+calibration:
+  pos_ET: [0.14982225501989826, -0.048118796065005576, -0.03482924194973972]
+  pos_WR: [0.033410320051684865, -0.5779185680332759, -0.129]
+  quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
+  quat_WR: [-0.0035406624036226648, 0.00401943913741378, -0.9999807593358486, 0.003128702105328147]
+targets:
+  pos_W:
+  - [0, 0.215, 0]
+  - [0.215, -0.215, 0]
+  - [-0.215, -0.215, 0]
+  - [0.322, 0, 0]
+  - [-0.322, -0.322, 0]

src/Controller.py

@@ -25,6 +25,7 @@ from control_msgs.msg import FollowJointTrajectoryActionResult
 
 # MOVEit
 import moveit_commander
+import moveit_msgs
 from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
 
 ## Our libraries
@@ -88,6 +89,10 @@ class Controller(object):
             self.robot = moveit_commander.RobotCommander()
             self.scene = moveit_commander.PlanningSceneInterface()
             self.group = moveit_commander.MoveGroupCommander('manipulator')
+            self.disp_traj_pub = \
+                    rospy.Publisher('/move_group/display_planned_path',
+                            moveit_msgs.msg.DisplayTrajectory,
+                            queue_size=20)
 
             # Traj completion feedback
             self.motion_feedback_sub = rospy.Subscriber(

src/KeyboardController.py

@@ -4,6 +4,7 @@ VERBOSE = True
 
 ## Python libraries
 from copy import deepcopy
+import time
 import sys
 
 ## 3rd party libraries
@@ -21,6 +22,7 @@ from geometry_msgs.msg import Pose
 from moveit_msgs.srv import GetPositionFK,GetPositionFKRequest
 from moveit_msgs.srv import GetPositionIK,GetPositionIKRequest
 from moveit_msgs.msg import MoveItErrorCodes
+from moveit_msgs.msg import DisplayTrajectory
 
 ## Our libraries
 from Controller import Controller,getR_ET
@@ -597,8 +599,31 @@ class KeyboardController(Controller):
                     gpose.position.x, gpose.position.y, gpose.position.z = p_r
                     gpose.orientation.x, gpose.orientation.y, \
                             gpose.orientation.z, gpose.orientation.w = q
-                    self.group.set_pose_target(gpose)
-                    self.group.go(wait=True)
+
+                    # compute path to next config
+                    waypoints = [self.group.get_current_pose().pose, gpose]
+                    plan, fraction = self.group.compute_cartesian_path(
+                            waypoints,
+                            0.01, # ee_step
+                            0)
+
+                    # display path on screen and confirm execution
+                    try:
+                        print("Displaying Trajectory to estimated point.")
+                        disp_traj = DisplayTrajectory()
+			disp_traj.trajectory_start = self.robot.get_current_state()
+			disp_traj.trajectory.append(plan)
+                        self.disp_traj_pub.publish(disp_traj)
+
+                        a = raw_input("Execute displayed Trajectory (y/n) ? ")
+                        if a == 'y':
+                            self.group.execute(plan, wait=True)
+                        else:
+                            print("OK, move manually to next pos.")
+
+		    except:
+                        Exception("Only calibrate one arm at a time")
+                time.sleep(1)
 
                 with keyboard.Listener(
                         on_release = lambda key:self._move_callback(key,configurations,params),