def execute(self, userdata):
if MasterClass.PAUSE_BETWEEN_STATES:
pause_func(self)
rospy.loginfo('Planning trajectory from gripper to object')
objectPose = tfx.pose(userdata.objectPose)
gripperPose = tfx.pose(userdata.gripperPose)
#objectPose.orientation = gripperPose.orientation
transBound = .006
rotBound = float("inf")
if Util.withinBounds(gripperPose, objectPose, transBound, rotBound, self.transFrame, self.rotFrame):
rospy.loginfo('Closing the gripper')
self.ravenArm.closeGripper(duration=self.gripperOpenCloseDuration)
userdata.vertAmount = .04
return 'reachedObject'
deltaPose = tfx.pose(Util.deltaPose(gripperPose, objectPose, self.transFrame, self.rotFrame))
endPose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose)
n_steps = int(self.stepsPerMeter * deltaPose.position.norm) + 1
poseTraj = self.ravenPlanner.getTrajectoryFromPose(self.ravenArm.name, endPose, n_steps=n_steps)
rospy.loginfo('deltaPose')
rospy.loginfo(deltaPose)
rospy.loginfo('n_steps')
rospy.loginfo(n_steps)
if poseTraj == None:
return 'failure'
userdata.poseTraj = poseTraj
return 'notReachedObject'
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
def executeDeltaPoseTrajectory(self, deltaPoses, startPose=None, block=True, speed=None, ignoreOrientation=False):
"""
Each deltaPose in deltaPoses is with respect to the startPose
Assuming deltaPoses and startPose in same frame (0_link)
"""
if startPose is None:
startPose = self.getGripperPose()
if startPose is None:
return
endPoses = [Util.endPose(startPose, deltaPose, self.commandFrame) for deltaPose in deltaPoses]
return self.executePoseTrajectory(endPoses, block=block, speed=speed, ignoreOrientation=ignoreOrientation)
def goToGripperPoseDelta(self, deltaPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False):
"""
Move by deltaPose
If startPose is not specified, default to current pose according to tf
(w.r.t. Link0)
"""
if startPose == None:
startPose = self.getGripperPose()
if startPose == None:
return
endPose = Util.endPose(startPose, deltaPose, self.commandFrame)
self.goToGripperPose(endPose, startPose=startPose, block=block, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation)
def execute(self, userdata):
if MasterClass.PAUSE_BETWEEN_STATES:
pause_func(self)
rospy.loginfo('Moving vertical with the object')
# move vertical with the object
deltaPose = tfx.pose([0,0,userdata.vertAmount]).msg.Pose()
endPose = Util.endPose(self.ravenArm.getGripperPose(), deltaPose)
endPoseTraj = self.ravenPlanner.getTrajectoryFromPose(self.ravenArm.name, endPose)
if endPoseTraj != None:
self.ravenArm.executePoseTrajectory(endPoseTraj)
return 'success'
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 executeStampedDeltaPoseTrajectory(self, stampedDeltaPoses, startPose=None, block=True):
"""
stampedDeltaPoses is a list of tfx poses with time stamps
each stampedDeltaPose is endPose - startPose, where
each endPose is different but the startPose is the same
This function is intended to be used where each
endPose is from the vision system and the one
startPose is the gripper position according to vision
"""
durations = []
prevTime = rospy.Time.now()
for stampedDeltaPose in stampedDeltaPoses:
durations.append(stampedDeltaPose.stamp - prevTime)
prevTime = stampedDeltaPose.stamp
if startPose == None:
startPose = self.getGripperPose()
if startPose == None:
return
startPose = Util.convertToFrame(tfx.pose(startPose), self.commandFrame)
endPoses = []
for deltaPose in stampedDeltaPoses:
endPoses.append(Util.endPose(startPose, deltaPose, self.commandFrame))
prevPose = None
for duration, endPose in zip(duration, endPoses):
self.goToGripperPose(endPose, startPose=prevPose, block=False, duration=duration)
prevPose = endPose
if block:
return self.blockUntilPaused()
return True
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()
