def Interpolate(robotname):
r = robot.robot(robotname)
# Open and close gripper
print("Starting gripper opening and closing")
r.open_gripper(80)
time.sleep(0.1)
r.close_gripper()
# Using move_cartesian_linear_interpolate
# Move to a different cartesian and rotation pose
start_pose = r.get_current_cartesian_position()
pose1 = r.get_current_cartesian_position()
pose1.position.y -= 0.03
end_pose = tfx.pose(pose1.as_tf()*tfx.transform(tfx.rotation_tb(30, -60, 30)))
print("Starting move_cartesian_frame_linear_interpolation")
r.move_cartesian_frame_linear_interpolation(start_pose, 0.01)
time.sleep(2)
r.move_cartesian_frame_linear_interpolation(end_pose, 0.01)
time.sleep(2)
r.move_cartesian_frame_linear_interpolation(start_pose, 0.01)
# Using move_cartesian_frame
print("Starting move_cartesian_frame")
r.move_cartesian_frame(start_pose, interpolate=True)
time.sleep(2)
r.move_cartesian_frame(end_pose, interpolate=True)
time.sleep(2)
r.move_cartesian_frame(start_pose, interpolate=True)
print start_pose
print end_pose
def compute_tissue_pose(self, nw, ne, sw):
x = -0.01
y = 0.1
z = 0.01
roll = 1.5 # in degrees
yaw = 2
pitch = 3.5
nw_position = np.hstack(np.array(nw.position))
ne_position = np.hstack(np.array(ne.position))
sw_position = np.hstack(np.array(sw.position))
u = sw_position - nw_position
v = ne_position - nw_position
self.tissue_length = norm(u)
self.tissue_width = norm(v)
u /= self.tissue_length
v /= self.tissue_width
origin = nw_position # origin of palpation board is the NW corner
w = np.cross(u, v)
u = np.cross(v, w) # to ensure that all axes are orthogonal
rotation_matrix = np.array([u,v,w]).transpose()
# offset = np.dot(rotation_matrix, np.array([-0.009, 0.100, 0.01]))
pose = tfx.pose(origin, rotation_matrix, frame=nw.frame)
pose = pose.as_tf()*tfx.transform([x,y,z])*tfx.transform(tfx.rotation_tb(0, 0, roll))*tfx.transform(tfx.rotation_tb(0, pitch, 0))*tfx.transform(tfx.rotation_tb(yaw, 0, 0))
self.tissue_pose = pose
self.tissue_width = 0.05
self.tissue_length = 0.025
return pose
def load_tissue_pose_from_registration_brick_pose(self):
# tissue pose offsets:
x = -0.048
y = 0.07
z = 0.006
rotation = 0.0
tissue_frame = pickle.load(open("registration_brick_pose.p", "rb"))
tissue_frame = tissue_frame.as_tf()*tfx.transform([x,y,z])*tfx.transform(tfx.rotation_tb(rotation, 0, 0))
self.tissue_pose = tfx.pose(tissue_frame)
self.tissue_width = 0.056
self.tissue_length = 0.028
def __init__(self):
self.ignore = False
self.currentCenters = []
self.newCenters = False
self.centerHistoryLength = tfx.duration(5)
self.centerHistory = {}
self.centerCombiningThreshold = 0.007
self.allocations = {}
self.allocationRadius = 0.03
self.waitForFoamDuration = tfx.duration(5)
self.orientation = tfx.rotation_tb(yaw=-90,pitch=90,roll=0)
self.lock = threading.RLock()
self.estPoseExclusionRadius = 0.035
self.estPose = {arm : None for arm in 'LR'}
self.estPoseSubs = { arm : rospy.Subscriber('/estimated_gripper_pose_%s' % arm, PoseStamped,functools.partial(self._estPoseCallback,arm)) for arm in 'LR'}
self.allocation_pub = rospy.Publisher('/foam_allocation',FoamAllocation)
self.sub = rospy.Subscriber('/foam_points', FoamPoints, self._foam_points_cb)
self._printThread = threading.Thread(target=self._printer)
self._printThread.daemon = True
#self._printThread.start()
self.marker_pub = rospy.Publisher('/foam_allocator_markers', MarkerArray)
self.event_pub = rospy.Publisher('/events', String)
self._publishThread = threading.Thread(target=self._publisher)
self._publishThread.daemon = True
self._publishThread.start()
def __init__(self, armNames, thread=True, simPlotting=False, stagePlotting=False):
"""
Hard-coding so only plans for first of armNames
assumes armNames is both raven arms
"""
if isinstance(armNames,basestring):
armNames = [armNames]
self.armNames = sorted(armNames)
self.refFrame = MyConstants.Frames.Link0
self.env = rave.Environment()
rospy.loginfo('Before loading model')
ravenFile = os.path.join(roslib.packages.get_pkg_subdir('RavenDebridement','models'),'myRaven.xml')
#ravenFile = '/home/gkahn/ros_workspace/RavenDebridement/models/myRaven.xml'
self.env.Load(ravenFile)
rospy.loginfo('After loading model')
self.robot = self.env.GetRobots()[0]
self.invKinArm = dict()
self.toolFrame = dict()
self.manipName = dict()
self.manip = dict()
self.manipJoints = dict()
self.raveJointNames = dict()
self.raveJointTypes = dict()
self.raveJointTypesToRos = dict()
self.rosJointTypesToRave = dict()
self.raveGrasperJointNames = dict()
self.raveGrasperJointTypes = dict()
self.trajRequest = dict()
self.trajEndJoints = dict()
self.trajEndGrasp = dict()
self.trajEndPose = dict()
self.trajStartJoints = dict()
self.trajStartGrasp = dict()
self.trajStartPose = dict()
self.trajSteps = dict()
self.jointTraj = dict() # for debugging
self.poseTraj = dict()
self.approachDir = dict()
activeDOFs = []
for armName in self.armNames:
self._init_arm(armName)
activeDOFs += self.raveJointTypes[armName]
self.robot.SetActiveDOFs(activeDOFs)
self.bsp = ravenbsp.RavenBSPWrapper(self.env)
# temp
self.activeArm = MyConstants.Arm.Right
self.inactiveArm = MyConstants.Arm.Left
self.bsp.set_manip_name(self.manip[self.activeArm].GetName())
self.bsp.set_start_sigma(np.eye(len(self.raveJointNames[self.activeArm]))*(.01**2))
self.bsp.set_link_name(self.toolFrame[self.activeArm])
self.bsp.set_sim_plotting(simPlotting)
self.bsp.set_stage_plotting(stagePlotting)
camera_pose = tfx.pose(( -0.032, -0.289, 0.255), tfx.rotation_tb(yaw=-85.0, pitch=0.7, roll=-118.7))
self.bsp.set_camera_pose(camera_pose.matrix)
self.currentState = None
rospy.Subscriber(MyConstants.RavenTopics.RavenState, RavenState, self._ravenStateCallback)
self.start_pose_pubs = dict((armName, rospy.Publisher('planner_%s_start' % armName,PoseStamped)) for armName in self.armNames)
self.end_pose_pubs = dict((armName, rospy.Publisher('planner_%s_end' % armName,PoseStamped)) for armName in self.armNames)
self.lock = threading.RLock()
if thread:
self.thread = threading.Thread(target=self.optimizeLoop)
self.thread.setDaemon(True)
self.thread.start()
def execute(self, userdata):
print "State: Start"
self.counter = StateTestClass.counter
while True:
rospy.sleep(0.1)
if (self.poses != None ):
if (len(self.poses) == 3):
break
if self.counter%2 == 0:
pose = self.poses[2]
self.publish_triangle_polygon(pose)
self.graspPoint = tfx.pose(pose, copy=True)
self.graspPoint = raven_util.convertToFrame(self.graspPoint, '/two_remote_center_link')
self.graspPoint.position.x += -0.0055
self.graspPoint.position.y += 0.012
self.graspPoint.position.z += 0.013
# rotation = tfx.rotation_tb(0,90,0) * tfx.rotation_tb(-30,0,0) * tfx.rotation_tb(0,0,5)
# rotation = tfx.rotation_tb(0,90,0) * tfx.rotation_tb(-30,0,0)
# self.graspPoint = self.graspPoint.as_tf()*rotation.as_pose()
rotation = tfx.rotation_tb(0,180,0)*tfx.rotation_tb(-90,0,0)
self.graspPoint.rotation = rotation
self.publisher.publish(self.graspPoint.msg.PoseStamped())
self.retractionStagingPose = tfx.pose(self.graspPoint, copy=True)
self.retractionStagingPose.position.z += 0.03
self.retractionStagingPose.position.x += 0.000
self.dropOffStagingPose = tfx.pose(self.retractionStagingPose, copy = True)
self.dropOffStagingPose.position.y += 0.01
self.dropOffStagingPose.position.x += 0.052
self.dropOffPose = tfx.pose(self.dropOffStagingPose, copy = True)
self.dropOffPose.position.z += - 0.010
self.counter += 1
else:
prev_retraction_staging_pose = self.retractionStagingPose
pose = self.poses[1]
self.publish_triangle_polygon(pose)
self.graspPoint = tfx.pose(pose, copy=True)
self.graspPoint = raven_util.convertToFrame(self.graspPoint, '/two_remote_center_link')
self.graspPoint.position.x += -0.0065
self.graspPoint.position.y += 0.013
self.graspPoint.position.z += 0.013
# rotation = tfx.rotation_tb(0,90,0) * tfx.rotation_tb(-30,0,0) * tfx.rotation_tb(0,0,5)
# rotation = tfx.rotation_tb(0,90,0) * tfx.rotation_tb(-30,0,0)
# self.graspPoint = self.graspPoint.as_tf()*rotation.as_pose()
rotation = tfx.rotation_tb(0,180,0)*tfx.rotation_tb(-90,0,0)
self.graspPoint.rotation = rotation
self.publisher.publish(self.graspPoint.msg.PoseStamped())
self.retractionStagingPose = tfx.pose(self.graspPoint, copy=True)
self.retractionStagingPose.position.z += 0.03
self.retractionStagingPose.position.x += 0.000
self.dropOffStagingPose = prev_retraction_staging_pose
self.dropOffStagingPose.position.x += 0.0065
self.dropOffPose = tfx.pose(self.dropOffStagingPose, copy = True)
self.dropOffPose.position.z += - 0.010
self.counter += 1
# rospy.loginfo('Execute Start')
# raw_input()
userdata.retractionStagingPose = self.retractionStagingPose
userdata.graspPoint = self.graspPoint
userdata.dropOffStagingPose = self.dropOffStagingPose
userdata.dropOffPose = self.dropOffPose
# IPython.embed()
# self.davinciArm.executeInterpolatedTrajectory(self.homePose)
return 'success'
