Commit 417cad0c authored by Jayant Khatkar's avatar Jayant Khatkar

use cartesian plaln and display and confirm (#18)

parent 5f991edc
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]
......@@ -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(
......
......@@ -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),
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment