def testSwitchPlaces(show=True):
#trajoptpy.SetInteractive(True)
rospy.init_node('testSwitchPlaces',anonymous=True)
rp = RavenPlanner(raven_constants.Arm.Both, thread=True)
#rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=raven_constants.Frames.Link0)
#leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=raven_constants.Frames.Link0)
leftCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Left]),raven_constants.Frames.Link0)
rightCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Right]),raven_constants.Frames.Link0)
rp.getTrajectoryPoseToPose(raven_constants.Arm.Left, leftCurrPose, rightCurrPose, n_steps=50)
rp.getTrajectoryPoseToPose(raven_constants.Arm.Right, rightCurrPose, leftCurrPose, n_steps=50)
#rp.getPoseTrajectory(raven_constants.Arm.Left, rightCurrPose, n_steps=50)
#rp.getPoseTrajectory(raven_constants.Arm.Right, leftCurrPose, n_steps=50)
#rp.getPoseTrajectory(raven_constants.Arm.Left, leftCurrPose+[0.01,0,0], n_steps=50)
#rp.getPoseTrajectory(raven_constants.Arm.Right, rightCurrPose-[0.01,0,0], n_steps=50)
print 'waiting'
rp.waitForTrajReady()
print 'woooooooo'
if rospy.is_shutdown():
return
#IPython.embed()
if not show:
return
rp.env.SetViewer('qtcoin')
leftPoseTraj = rp.poseTraj[raven_constants.Arm.Left]
rightPoseTraj = rp.poseTraj[raven_constants.Arm.Right]
for left, right in zip(leftPoseTraj,rightPoseTraj):
if rospy.is_shutdown():
break
rp.updateOpenraveJoints('L', rp.getJointsFromPose('L', left, rp.getCurrentGrasp('L')), grasp=rp.getCurrentGrasp('L'))
rp.updateOpenraveJoints('R', rp.getJointsFromPose('R', right, rp.getCurrentGrasp('R')), grasp=rp.getCurrentGrasp('R'))
rospy.loginfo('Press enter to go to next step')
raw_input()
return
leftTraj = rp.jointTraj[raven_constants.Arm.Left]
rightTraj = rp.jointTraj[raven_constants.Arm.Right]
for left, right in zip(leftTraj,rightTraj):
if rospy.is_shutdown():
break
rp.updateOpenraveJoints('L', left)
rp.updateOpenraveJoints('R', right)
rospy.loginfo('Press enter to go to next step')
raw_input()
def setStartAndEndPose(self, armName, startPose, endPose, **kwargs):
startGrasp = kwargs.get('startGrasp',kwargs.get('grasp',0))
endGrasp = kwargs.get('endGrasp',kwargs.get('grasp',0))
startJoints = self.getJointsFromPose(armName, startPose, grasp=startGrasp)
endJoints = self.getJointsFromPose(armName, endPose, grasp=endGrasp)
self.trajStartGrasp[armName] = startGrasp
self.trajEndGrasp[armName] = endGrasp
startPose = raven_util.convertToFrame(tfx.pose(startPose), raven_constants.Frames.Link0)
self.trajStartPose[armName] = startPose
endPose = raven_util.convertToFrame(tfx.pose(endPose), raven_constants.Frames.Link0)
self.trajEndPose[armName] = endPose
self.trajStartJoints[armName] = startJoints
self.trajEndJoints[armName] = endJoints
def testFromAbove(show=True):
trajoptpy.SetInteractive(True)
rospy.init_node('testFromAbove',anonymous=True)
rp = RavenPlanner([raven_constants.Arm.Left, raven_constants.Arm.Right], thread=True)
leftCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Left]),raven_constants.Frames.Link0)
rightCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Right]),raven_constants.Frames.Link0)
rp.getTrajectoryFromPose(raven_constants.Arm.Left, leftCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1]), block=False)
rp.getTrajectoryFromPose(raven_constants.Arm.Right, rightCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1]))
#IPython.embed()
print 'waiting'
rp.waitForTrajReady()
print 'woooooooo'
if rospy.is_shutdown():
return
#IPython.embed()
if not show:
return
rp.env.SetViewer('qtcoin')
leftPoseTraj = rp.poseTraj[raven_constants.Arm.Left]
rightPoseTraj = rp.poseTraj[raven_constants.Arm.Right]
leftTraj = rp.jointTraj[raven_constants.Arm.Left]
rightTraj = rp.jointTraj[raven_constants.Arm.Right]
for right in rightTraj:
if rospy.is_shutdown():
break
rp.updateOpenraveJoints('L', left)
rp.updateOpenraveJoints('R', right)
rospy.loginfo('Press enter to go to next step')
raw_input()
IPython.embed()
def getGripperPose(self,frame=raven_constants.Frames.Link0):
"""
Returns gripper pose w.r.t. frame
geometry_msgs.msg.Pose
"""
pose = tfx.pose([0,0,0],frame=self.toolFrame)
return tfx.convertToFrame(pose, self.commandFrame, pose.frame, wait=10)
gripperPose = self.ravenController.currentPose
if gripperPose != None:
gripperPose = raven_util.convertToFrame(tfx.pose(gripperPose), frame)
return gripperPose
def goToGripperPose(self, endPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False):
"""
Move to endPose
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
startPose = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame)
endPose = raven_util.convertToFrame(tfx.pose(endPose), self.commandFrame)
if ignoreOrientation:
endPose.orientation = startPose.orientation
self.ravenController.goToPose(endPose, start=startPose, duration=duration, speed=speed)
if block:
return self.blockUntilPaused()
return True
def getJointsFromPose(self, armName, pose, grasp, quiet=False):
"""
Calls IK server and returns a dictionary of {jointType : jointPos}
jointType is from raven_2_msgs.msg.Constants
jointPos is position in radians
Needs to return finger1 and finger2
"""
pose = raven_util.convertToFrame(tfx.pose(pose), self.refFrame)
joints = kin.invArmKin(armName, pose, grasp)
if joints is None:
rospy.loginfo('IK failed!')
if quiet:
return None
else:
raise RuntimeError("IK failed!")
return joints
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 = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame)
endPoses = []
for deltaPose in stampedDeltaPoses:
endPoses.append(raven_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
