def execAndRecordTraj(arm=Constants.Arm.Right):
rospy.init_node('execAndRecordTraj',anonymous=True)
rospy.sleep(1)
ravenArm = RavenArm(arm)
ravenPlanner = RavenPlanner(arm)
record = Test(arm)
rospy.sleep(2)
ravenArm.start()
rospy.loginfo('Classes are initialized, press enter')
raw_input()
startPose = tfx.pose(ravenArm.getGripperPose())
x = .01
y = .06
z = .05
deltaPoses = []
deltaPoses.append(tfx.pose([x,0, -z],frame=Constants.Frames.Link0))
#deltaPoses.append(tfx.pose([0,-y, 0],frame=Constants.Frames.Link0))
deltaPoses.append(tfx.pose([-x,0,z],frame=Constants.Frames.Link0))
#deltaPoses.append(tfx.pose([0,y, 0],frame=Constants.Frames.Link0))
#record.startRecording()
while not rospy.is_shutdown():
for deltaPose in deltaPoses:
startPose = tfx.pose(ravenArm.getGripperPose())
endPose = tfx.pose(Util.endPose(startPose, deltaPose))
ravenArm.goToGripperPose(endPose)
rospy.sleep(4)
record.stopRecording()
rospy.loginfo('Press enter to print and exit')
raw_input()
ravenArm.stop()
record.printPoses()
IPython.embed()
def invKinClient():
rospy.init_node('inv_kin_client',anonymous=True)
rospy.sleep(4)
ravenArm = RavenArm(MyConstants.Arm.Right)
rospy.sleep(4)
start = tfx.pose(ravenArm.getGripperPose())
end = start + [0,-.05, 0]
try:
rospy.wait_for_service('inv_kin_server',timeout=5)
inv_kin_service = rospy.ServiceProxy('inv_kin_server', InvKinSrv)
arm = Constants.ARM_TYPE_GREEN
pose = tfx.pose(ravenArm.getGripperPose())
grasp = ravenArm.ravenController.currentGrasp
rospy.loginfo('Find ik for ' + str(pose))
respStart = inv_kin_service(arm, grasp, start)
respEnd = inv_kin_service(arm, grasp, end)
rospy.loginfo('Called service')
except (rospy.ServiceException, rospy.ROSException) as e:
print "Service call failed: %s"%e
return
startJoints = []
for joint in respStart.joints:
if joint.type in RavenPlanner.rosJointTypes:
startJoints.append(joint.position)
#print("({0},{1})".format(joint.type,joint.position))
endJoints = []
for joint in respEnd.joints:
if joint.type in RavenPlanner.rosJointTypes:
endJoints.append(joint.position)
#print("({0},{1})".format(joint.type,joint.position))
startJoints = np.array(startJoints)
endJoints = np.array(endJoints)
timeSteps = 50
for i in range(timeSteps):
print(list(startJoints + (float(i)/float(timeSteps)*(endJoints-startJoints))))
class GenerateTraj():
def __init__(self, arm=MyConstants.Arm.Right):
self.arm = arm
self.ravenArm = RavenArm(self.arm)
self.ravenPlanner = RavenPlanner(self.arm)
def generateTraj(self, deltaPose):
startPose = tfx.pose(self.ravenArm.getGripperPose())
deltaPose = tfx.pose(deltaPose)
#code.interact(local=locals())
startJoints = {0:0.51091998815536499,
1:1.6072717905044558,
2:-0.049991328269243247,
4:0.14740140736103061,
5:0.10896652936935426,
8:-0.31163200736045837}
endJoints = {0:0.45221099211619786,
1:2.2917657932075581,
2:-0.068851854076958902,
4:0.44096283117933965,
5:0.32085205361054148,
8:-0.82079765727524379}
endPose = Util.endPose(startPose, deltaPose)
#endPose = startPose
#IPython.embed()
jointTraj = self.ravenPlanner.getTrajectoryFromPose(endPose, startJoints=startJoints, n_steps=15)
"""
n_steps = 15
self.ravenPlanner.updateOpenraveJoints(startJoints)
endJointPositions = endJoints.values()
request = Request.joints(n_steps, self.ravenPlanner.manip.GetName(), endJointPositions)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.ravenPlanner.env)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
# check trajectory safety
from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs()
if not traj_is_safe(result.GetTraj(), self.ravenPlanner.robot):
rospy.loginfo('Trajopt trajectory is not safe. Trajopt failed!')
rospy.loginfo('Still returning trajectory')
#return
IPython.embed()
return self.ravenPlanner.trajoptTrajToDicts(result.GetTraj())
"""
return jointTraj
class GripperFOVClass():
def __init__(self, armName):
print('Initializing GripperFOVClass')
self.armName = armName
self.ravenArm = RavenArm(self.armName)
self.imageDetector = ImageDetector()
self.camFrame = '/left_BC'
self.desiredOrientation = tfx.tb_angles(90,0,0) # in self.camFrame
self.posFrame = MyConstants.Frames.Link0
self.rotFrame = MyConstants.Frames.RightTool if self.armName == MyConstants.Arm.Right else MyConstants.Frames.LeftTool
self.pub = rospy.Publisher('orig_gripper_pose',PoseStamped)
self.file = open('/tmp/GripperFOVTest.txt','w')
print('Initialized GripperFOVClass')
def currentPose(self):
currTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), self.camFrame)
currDetectedPose = Util.convertToFrame(self.imageDetector.getGripperPose(self.armName), self.camFrame)
currPoseInCamera = tfx.pose(currTfPose.position, currDetectedPose.orientation)
return currPoseInCamera
def homePose(self):
homePose = self.currentPose()
homePose.orientation = self.desiredOrientation
return homePose
def findNewGripper(self, timeout=1):
timeout = Util.Timeout(timeout)
self.imageDetector.ignoreOldGripper(self.armName)
timeout.start()
while not timeout.hasTimedOut():
if self.imageDetector.hasFoundNewGripper(self.armName):
return tfx.pose(self.imageDetector.getGripperPose(self.armName))
return None
def rotateUntilUnseen(self, axis, rotateBy):
firstPose = gripperPose = Util.convertToFrame(self.findNewGripper(), MyConstants.Frames.Link0)
firstTfPose = Util.convertToFrame(self.ravenArm.getGripperPose(), MyConstants.Frames.Link0)
self.pub.publish(firstPose.msg.PoseStamped())
if axis == 'pitch':
tb_rotate = tfx.tb_angles(0,rotateBy,0)
origAngle = tfx.tb_angles(firstPose.orientation).pitch_deg
elif axis == 'roll':
tb_rotate = tfx.tb_angles(0,0,rotateBy)
origAngle = tfx.tb_angles(firstPose.orientation).roll_deg
else:
return 0.0
totalRotation = 0.0
rotatePose = tfx.pose([0,0,0], tb_rotate)
lastOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation
lastPose = None
while gripperPose is not None:
#gripperPose = Util.convertToFrame(gripperPose, self.rotFrame)
#tfPose = tfx.pose([0,0,0],frame=self.rotFrame)
#tapeRotation = gripperPose.orientation
#tfRotation = tfPose.orientation
#IPython.embed()
#return
#tfGripperPose = tfx.pose(self.ravenArm.getGripperPose())
#gripperPose = Util.convertToFrame(gripperPose, tfGripperPose.frame)
#endPose = tfx.pose(tfGripperPose.position, rotatePose.orientation.matrix * gripperPose.orientation.matrix)
#self.ravenArm.goToGripperPose(endPose, duration=1)
#endPose = Util.endPose(gripperPose,tfx.pose([0,0,0],rotatePose.orientation, frame=self.camFrame),frame=MyConstants.Frames.Link0)
#deltaPose = tfx.pose([0,0,0], endPose.orientation)
#self.ravenArm.goToGripperPoseDelta(deltaPose, duration=1)
tfGripperPose = tfx.pose(self.ravenArm.getGripperPose())
tfGripperPose = Util.convertToFrame(tfGripperPose, self.camFrame)
endPose = Util.endPose(tfGripperPose, tfx.pose([0,0,0],tb_rotate,frame=self.camFrame))
endPose = Util.convertToFrame(endPose, MyConstants.Frames.Link0)
endPose.position = firstTfPose.position
self.ravenArm.goToGripperPose(endPose, duration=1)
print('Rotating...')
rospy.sleep(.25)
#self.ravenArm.goToGripperPoseDelta(rotatePose, duration=1)
totalRotation += rotateBy
#print('Total rotation = {0}'.format(totalRotation))
currOrientation = tfx.pose(self.ravenArm.getGripperPose()).orientation
if lastOrientation is not None:
angleBetween = Util.angleBetweenQuaternions(lastOrientation.msg.Quaternion(), currOrientation.msg.Quaternion())
#print('angleBetween = {0}'.format(angleBetween))
if angleBetween < rotateBy*.5:
self.file.write('Hit physical limit\n')
print('Hit physical limit')
break
lastOrientation = currOrientation
rospy.sleep(.5)
lastPose = Util.convertToFrame(gripperPose, self.camFrame)
gripperPose = self.findNewGripper()
if lastPose is not None:
firstPose = Util.convertToFrame(firstPose, self.camFrame)
if axis == 'pitch':
firstPitch = tfx.tb_angles(firstPose.orientation).pitch_deg
lastPitch = tfx.tb_angles(lastPose.orientation).pitch_deg
return copysign(fabs(lastPitch-firstPitch),rotateBy)
elif axis == 'roll':
firstRoll = tfx.tb_angles(firstPose.orientation).roll_deg
lastRoll = tfx.tb_angles(lastPose.orientation).roll_deg
return copysign(fabs(lastRoll-firstRoll),rotateBy)
return totalRotation
def runTest(self):
self.ravenArm.start()
rospy.sleep(2)
currPoseInCamera = self.currentPose()
homePose = self.homePose()
deltaPose = Util.deltaPose(currPoseInCamera, homePose, self.posFrame, self.rotFrame)
self.ravenArm.goToGripperPoseDelta(deltaPose, duration=2)
# self.ravenArm.goToGripperPoseDelta(tfx.pose([0,0,0],tfx.tb_angles(0,30,0)),duration=2)
#rotatePose = tfx.pose([0,0,0],tfx.tb_angles(0,45,0))
#IPython.embed()
#return
# z must always stay the same
z = currPoseInCamera.position.z
xMove = .03
yMove = .01
gridMoves = [[-xMove, 0, 0],
[0, yMove, 0],
[xMove, 0, 0],
[0, -yMove, 0]]
rotationAxes = ['pitch', 'roll']
for gridMove in gridMoves:
print('Go to next place on grid, press enter')
raw_input()
deltaPose = Util.deltaPose(self.currentPose(), self.homePose()+gridMove, self.posFrame, self.rotFrame)
homePose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose)
self.ravenArm.goToGripperPose(homePose)
self.file.write('Home pose {0}\n'.format(homePose))
print('Home pose {0}'.format(homePose))
for axis in rotationAxes:
if rospy.is_shutdown():
return
rotateBy = 5.0
self.file.write('Rotating in positive direction on {0} axis\n'.format(axis))
print('Rotate in positive direction on {0} axis'.format(axis))
#raw_input()
maxRotation = self.rotateUntilUnseen(axis, rotateBy)
self.ravenArm.goToGripperPose(homePose, duration=1)
rospy.sleep(1)
self.file.write('Rotating in negative direction on {0} axis\n'.format(axis))
print('Rotate in negative direction on {0} axis'.format(axis))
#raw_input()
minRotation = self.rotateUntilUnseen(axis, -rotateBy)
self.ravenArm.goToGripperPose(homePose, duration=1)
rospy.sleep(1)
self.file.write('{0} range of motion is ({1},{2})\n'.format(axis,minRotation,maxRotation))
print('{0} range of motion is ({1},{2})'.format(axis,minRotation,maxRotation))
self.file.close()
self.ravenArm.stop()
