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 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 testSwitchPlaces(show=True):
#trajoptpy.SetInteractive(True)
rospy.init_node('testSwitchPlaces',anonymous=True)
rp = RavenPlanner([MyConstants.Arm.Left,MyConstants.Arm.Right], thread=True)
#rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=MyConstants.Frames.Link0)
#leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=MyConstants.Frames.Link0)
leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0)
rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0)
#rp.getTrajectoryPoseToPose(MyConstants.Arm.Left, leftCurrPose, rightCurrPose, n_steps=50)
#rp.getTrajectoryPoseToPose(MyConstants.Arm.Right, rightCurrPose, leftCurrPose, n_steps=50)
rospy.loginfo('Press enter to get poses')
raw_input()
rp.getPoseTrajectory(MyConstants.Arm.Left, leftCurrPose+[0.01,0,0], n_steps=50)
rp.getPoseTrajectory(MyConstants.Arm.Right, rightCurrPose-[0.01,0,0], n_steps=50)
#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[MyConstants.Arm.Left]
rightPoseTraj = rp.poseTraj[MyConstants.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[MyConstants.Arm.Left]
rightTraj = rp.jointTraj[MyConstants.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 testRotation():
rospy.init_node('ar_image_detection')
imageDetector = ARImageDetector()
listener = tf.TransformListener()
tf_br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
if imageDetector.hasFoundGripper(Constants.Arm.Left):
obj = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped()
gripper = imageDetector.getGripperPose(Constants.Arm.Left)
print('gripper')
print(gripper)
# obj_tb = tfx.tb_angles(obj.pose.orientation)
gripper_tb = tfx.tb_angles(gripper.pose.orientation)
print "gripper ori", gripper_tb
obj_tb = tfx.tb_angles(imageDetector.normal)
print "obj ori", obj_tb
pt = gripper.pose.position
ori = imageDetector.normal
tf_br.sendTransform((pt.x, pt.y, pt.z), (ori.x, ori.y, ori.z, ori.w),
gripper.header.stamp, '/des_pose', Constants.AR.Frames.Base)
between = Util.angleBetweenQuaternions(ori, gripper_tb.msg)
print('Angle between')
print(between)
quat = tft.quaternion_multiply(gripper_tb.quaternion, tft.quaternion_inverse(obj_tb.quaternion))
print 'new', tfx.tb_angles(quat)
#rot = gripper_tb.rotation_to(ori)
rot = gripper_tb.rotation_to(obj_tb)
print('Rotation from gripper to obj')
print(rot)
deltaPoseTb = tfx.pose(Util.deltaPose(gripper, obj)).orientation
print('deltaPose')
print(deltaPoseTb)
deltaPose = tfx.pose([0,0,0], deltaPoseTb).msg.PoseStamped()
time = listener.getLatestCommonTime('0_link', 'tool_L')
deltaPose.header.stamp = time
deltaPose.header.frame_id = '0_link'
deltaPose = listener.transformPose('tool_L', deltaPose)
print "transformed", tfx.tb_angles(deltaPose.pose.orientation)
endQuatMat = gripper_tb.matrix * rot.matrix
print 'desOri', tfx.tb_angles(endQuatMat)
rospy.sleep(1)
def testSwitchPlaces(armNames=[MyConstants.Arm.Left,MyConstants.Arm.Right],fixedPose=False):
rospy.init_node('testSwitchPlaces',anonymous=True)
rp = RavenPlanner(armNames)
rospy.sleep(2)
leftStartJoints = None
rightStartJoints = None
if fixedPose:
rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=MyConstants.Frames.Link0)
leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=MyConstants.Frames.Link0)
leftStartJoints = rp.getJointsFromPose(MyConstants.Arm.Left, leftCurrPose, grasp=0)
rightStartJoints = rp.getJointsFromPose(MyConstants.Arm.Right, rightCurrPose, grasp=0)
else:
leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0)
rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0)
#trajoptpy.SetInteractive(True)
if fixedPose:
rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, endGrasp=0, startJoints=leftStartJoints, debug=True)
rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, endGrasp=0, startJoints=rightStartJoints, debug=True)
else:
rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, leftStartJoints, debug=True)
rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, rightStartJoints, debug=True)
"""
for index in range(len(armNames)):
armName = armNames[index]
otherArmName = armNames[(index+1)%len(armNames)]
desPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[otherArmName]),MyConstants.Frames.Link0)
rp.getTrajectoryFromPose(armName, desPose, debug=True)
"""
rp.getTrajectoryFromPoseThread(once=True)
# while rp.trajRequest[armNames[0]] and rp.trajRequest[armNames[1]] and not rospy.is_shutdown():
# rospy.sleep(.05)
leftTraj = rp.jointTraj[MyConstants.Arm.Left]
rightTraj = rp.jointTraj[MyConstants.Arm.Right]
rp.env.SetViewer('qtcoin')
for left, right in zip(leftTraj,rightTraj):
rospy.loginfo('Press enter to go to next step')
raw_input()
rp.updateOpenraveJoints('L', left)
rp.updateOpenraveJoints('R', right)
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 = Util.convertToFrame(tfx.pose(startPose), MyConstants.Frames.Link0)
self.trajStartPose[armName] = startPose
endPose = Util.convertToFrame(tfx.pose(endPose), MyConstants.Frames.Link0)
self.trajEndPose[armName] = endPose
self.trajStartJoints[armName] = startJoints
self.trajEndJoints[armName] = endJoints
def getGripperPoseEstimate(self, armName):
if not self.hasFoundGripper(armName):
return False
if armName == Constants.Arm.Left:
lastGripperPose = self.leftGripperPose
lastTfPose = self.leftGripperPoseTf
currTfPose = Util.convertToFrame(tfx.pose([0,0,0],frame=Constants.Frames.LeftTool), Constants.Frames.Link0)
else:
lastGripperPose = self.rightGripperPose
lastTfPose = self.rightGripperPoseTf
currTfPose = Util.convertToFrame(tfx.pose([0,0,0],frame=Constants.Frames.RightTool), Constants.Frames.Link0)
deltaTf = Util.deltaPose(currTfPose, lastTfPose)
def testCheckPickup(armName=Constants.Arm.Right):
rospy.init_node('testCheckPickup',anonymous=True)
imageDetector = ImageDetector()
rospy.sleep(4)
rospy.loginfo('Press enter to get {0} gripper point'.format(armName))
raw_input()
gripperPose = imageDetector.getGripperPose(armName)
convGripperPose = tfx.pose(Util.convertToFrame(gripperPose, Constants.Frames.Link0))
lastGripperPoint = convGripperPose.position.msg.PointStamped()
lastGripperPoint.point.z += 0
rospy.loginfo('Press enter to thresh for red at last gripper point')
raw_input()
serviceName = '{0}_{1}'.format(Constants.Services.isFoamGrasped, armName)
try:
rospy.wait_for_service(serviceName, timeout=5)
foamGraspedService = rospy.ServiceProxy(serviceName, ThreshRed)
isFoamGrasped = foamGraspedService(lastGripperPoint).output
if isFoamGrasped == 1:
rospy.loginfo('Found red piece!')
else:
rospy.loginfo('Did not find red piece!')
except e:
rospy.loginfo('Thresh service failed: {0}'.format(e))
rospy.spin()
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 = Util.convertToFrame(tfx.pose(pose), self.refFrame)
try:
rospy.loginfo('Waiting for IK server...')
rospy.wait_for_service('inv_kin_server',timeout=5)
inv_kin_service = rospy.ServiceProxy('inv_kin_server', InvKinSrv)
rospy.loginfo('Calling the IK server')
resp = inv_kin_service(self.invKinArm[armName], grasp, pose.msg.Pose())
rospy.loginfo('IK success!')
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.loginfo("IK server failure: %s"%e)
if quiet:
return
else:
raise e
return dict((joint.type, joint.position) for joint in resp.joints)
def tapeCallbackRight(self, msg):
self.newRightGripperPose = True
self.tapeMsg = msg
self.rightGripperPose = msg
self.gripperPoseIsEstimate = False
self.rightGripperPoseTf = Util.convertToFrame(tfx.pose([0,0,0],frame=Constants.Frames.RightTool), Constants.Frames.Link0)
def getTrajectoryFromPose(self, armName, endPose, endGrasp=None, startJoints=None, reqType=Request.Type.Joints, n_steps=50, debug=False):
endPose = Util.convertToFrame(tfx.pose(endPose), MyConstants.Frames.Link0)
self.trajEndPose[armName] = endPose
self.trajEndJoints[armName] = self.getJointsFromPose(armName, endPose, grasp=endGrasp)
if self.trajEndJoints[armName] == None:
return None
if startJoints == None:
self.trajStartJoints[armName] = self.currentJoints[armName]
print 'cj', self.currentJoints[armName]
else:
self.trajStartJoints[armName] = startJoints
self.trajSteps[armName] = n_steps
self.trajReqType = reqType
self.trajRequest[armName] = True
if debug:
return
print 'waiting for traj'
while self.trajRequest[armName] and not rospy.is_shutdown():
rospy.sleep(.05)
return self.poseTraj[armName]
def posesToDeltaPoses(self, poseList):
"""
p0 -> p1 -> p2 -> ....
to
delta(p0->p1) -> delta(p0->p2) -> delta(p0->p3)...
"""
startPose = poseList[0]
return [Util.deltaPose(startPose, pose) for pose in poseList[1:]]
def testFromAbove(show=True):
trajoptpy.SetInteractive(True)
rospy.init_node('testFromAbove',anonymous=True)
rp = RavenPlanner([MyConstants.Arm.Left, MyConstants.Arm.Right], thread=True)
leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0)
rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0)
rp.getTrajectoryFromPose(MyConstants.Arm.Left, leftCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1]), block=False)
rp.getTrajectoryFromPose(MyConstants.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[MyConstants.Arm.Left]
rightPoseTraj = rp.poseTraj[MyConstants.Arm.Right]
leftTraj = rp.jointTraj[MyConstants.Arm.Left]
rightTraj = rp.jointTraj[MyConstants.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 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 testMoveActiveArm(show=True):
rospy.init_node('testMoveActiveArm',anonymous=True)
rp = RavenPlannerBSP([MyConstants.Arm.Left,MyConstants.Arm.Right],thread=True,simPlotting=True,stagePlotting=True)
rospy.sleep(4)
leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0)
rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0)
if rp.activeArm == MyConstants.Arm.Left:
rp.getTrajectoryFromPose(MyConstants.Arm.Left, leftCurrPose-[0,0.05,0], n_steps=50, block=False)
rp.getTrajectoryFromPose(MyConstants.Arm.Right, rightCurrPose, n_steps=50, block=False)
else:
rp.getTrajectoryFromPose(MyConstants.Arm.Left, leftCurrPose, n_steps=50, block=False)
rp.getTrajectoryFromPose(MyConstants.Arm.Right, rightCurrPose-[0,0.05,0], n_steps=50, block=False)
print 'waiting'
rp.waitForTrajReady()
print 'woooooooo'
if rospy.is_shutdown():
return
if not show:
return
rp.env.SetViewer('qtcoin')
leftTraj = rp.jointTraj[MyConstants.Arm.Left]
rightTraj = rp.jointTraj[MyConstants.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()
IPython.embed()
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 updateOpenraveJoints(self, armName, joints1=None, grasp=None):
"""
Updates the openrave raven model using rosJoints
rosJoints is dictionary of {jointType : jointPos}
jointType is from raven_2_msgs.msg.Constants
jointPos is position in radians
Updates based on: shoulder, elbow, insertion,
rotation, pitch, yaw, grasp (self.currentGrasp)
"""
rosJoints = joints1
if rosJoints is None:
rosJoints = self.getCurrentJoints(armName)
if rosJoints is None:
return
if grasp is None:
grasp = self.getCurrentGrasp(armName)
if grasp is None:
grasp = 0.
raveJointTypes = []
jointPositions = []
for rosJointType, jointPos in rosJoints.items():
if self.rosJointTypesToRave[armName].has_key(rosJointType):
raveJointType = self.rosJointTypesToRave[armName][rosJointType]
raveJointTypes.append(raveJointType)
# since not a hard limit, must do this
if rosJointType == Constants.JOINT_TYPE_ROTATION:
lim = self.robot.GetJointFromDOFIndex(raveJointType).GetLimits()
limLower, limUpper = lim[0][0], lim[1][0]
jointPos = Util.setWithinLimits(jointPos, limLower, limUpper, 2*pi)
jointPositions.append(jointPos)
# for opening the gripper
raveJointTypes += self.raveGrasperJointTypes[armName]
if armName == MyConstants.Arm.Left:
jointPositions += [grasp/2, grasp/2]
else:
jointPositions += [grasp/2, -grasp/2]
self.robot.SetJointValues(jointPositions, raveJointTypes)
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 getGripperPose(self,frame=MyConstants.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 = Util.convertToFrame(tfx.pose(gripperPose), frame)
return gripperPose
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 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 = Util.convertToFrame(tfx.pose(startPose), self.commandFrame)
endPose = 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 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 moveNearObject(self, failMethod=None, successMethod=None):
"""
Goes near the object
Returns servoToObject
"""
failMethod = failMethod or self.moveNearObject
successMethod = successMethod or self.servoToObject
rospy.loginfo('Moving near the object point')
# go to near the object point
deltaPose = Util.deltaPose(self.gripperPose, self.objectPose, self.transFrame, self.rotFrame)
deltaPose.position.z += .03
self.ravenArm.goToGripperPoseDelta(deltaPose, speed=self.openLoopSpeed)
return successMethod
def test():
rospy.init_node('testGripperPoseEstimator',anonymous=True)
rospy.sleep(2)
arms = Constants.Arm.Both
gpe = GripperPoseEstimator(arms)
rospy.sleep(2)
estimatePub = dict()
truthPub = dict()
prevPoseAndIsEstimate = dict()
for arm in arms:
truthPub[arm] = rospy.Publisher('truth_gripper_pose_%s'%arm,PoseStamped)
printEvery = Util.Timeout(1)
printEvery.start()
while not rospy.is_shutdown():
for arm in arms:
truthPose = gpe.truthPose.get(arm)
estimatedPoseisEstimate = gpe.estimatedPose.get(arm)
if truthPose is not None:
#print 'Publishing truthPose %s' % arm
truthPub[arm].publish(truthPose.msg.PoseStamped())
if estimatedPoseisEstimate is not None:
estimatedPose, isEstimate = estimatedPoseisEstimate
#print 'Publish estimatedPose %s' % arm
if estimatedPoseisEstimate is not None and prevPoseAndIsEstimate.has_key(arm):
prevPose, prevIsEstimate = prevPoseAndIsEstimate[arm]
if prevIsEstimate is True and isEstimate is False:
deltaPose = Util.deltaPose(prevPose, estimatedPose)
print 'deltaPose between last estimate and current truth'
print deltaPose
#print 'truthPose_{0}: {1}'.format(arm,truthPose)
#print 'isEstimate: {0}'.format(isEstimate)
#print 'estimatedPose_{0}: {1}'.format(arm,estimatedPose)
if estimatedPoseisEstimate is not None:
prevPoseAndIsEstimate[arm] = (estimatedPose, isEstimate)
rospy.sleep(.02)
def foamCallback(self, msg):
time = self.listener.getLatestCommonTime(Constants.AR.Frames.Base, msg.header.frame_id)
msg.header.stamp = time
self.objectPoint = self.listener.transformPoint(Constants.AR.Frames.Base,msg)
"""
tfxMsg = tfx.point(msg)
tf_msgframe_to_base = tfx.lookupTransform(Constants.AR.Frames.Base, tfxMsg.frame, wait=10)
tfxMsg = tf_msgframe_to_base * tfxMsg
self.objectPoint = tfxMsg.msg.PointStamped()
"""
pose = self.getObjectPose()
pos = pose.pose.position
ori = pose.pose.orientation
self.tf_br.sendTransform((pos.x, pos.y, pos.z), (ori.x, ori.y, ori.z, ori.w),
rospy.Time.now(), 'object_frame', pose.header.frame_id)
marker = Util.createMarker(self.getObjectPose(), 0)
self.objPublisher.publish(marker)
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 _objectPoseCallback(self, msg):
"""
temp callback for creating box in between object and gripper
"""
box = rave.RaveCreateKinBody(self.env,'')
box.SetName('testbox')
box.InitFromBoxes(np.array([[0,0,0,0.01,0.01,0.05]]),True)
pose = Util.convertToFrame(msg, MyConstants.Frames.Link0)
#pose.position.x += .01
poseMatrix = np.array(pose.matrix)
#convPoseMatrix = transformRelativePoseForIk(self.robot.GetManipulators()[0],poseMatrix,pose.frame,'world')
poseMatInRef = poseMatrix
refLinkName = pose.frame
targLinkName = 'world'
# ref -> world
refFromWorld = self.robot.GetLink(refLinkName).GetTransform()
# target -> world
targFromWorld = self.robot.GetLink(targLinkName).GetTransform()
# target -> ref
targFromRef = np.dot(np.linalg.inv(targFromWorld), refFromWorld)
poseMatInTarg = np.dot(targFromRef, poseMatInRef)
convPose = tfx.pose(poseMatInTarg, frame=targLinkName)
convPoseMatrix = convPose.matrix
convPoseMatrix[:3,:3] = np.identity(3)
box.SetTransform(np.array(convPoseMatrix))
rospy.loginfo('POSE.POSITION.X {0}'.format(pose.position.x))
if pose.position.x < -.05:
p = tfx.pose(self.manip[MyConstants.Arm.Right].GetEndEffectorTransform())
p.position.x += .03
m = p.matrix
m[:3,:3] = np.eye(3)
box.SetTransform(np.array(m))
self.env.Add(box,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 = 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 tapeCallback(self, msg):
if not self.record:
return
self.recordCount += 1
rospy.loginfo('Received tape pose number {0}'.format(self.recordCount))
tapePose = tfx.pose(msg)
stamp = tapePose.stamp
tf_tape_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, tapePose.frame, wait=4)
tapePose = tf_tape_to_link0 * tapePose
tfPose = tfx.pose([0,0,0], frame=self.toolFrame, stamp=stamp)
tf_tfpose_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, tfPose.frame, wait=4)
tfPose = tf_tfpose_to_link0 * tfPose
deltaPose = tfx.pose(Util.deltaPose(tfPose, tapePose))
self.tapePoses.append(tapePose)
self.tfPoses.append(tfPose)
self.deltaPoses.append(deltaPose)
def testFK():
rospy.init_node('test_IK',anonymous=True)
rp = RavenPlanner(MyConstants.Arm.Both)
rp.env.SetViewer('qtcoin')
rospy.sleep(2)
leftPose = rp.getCurrentPose(MyConstants.Arm.Left)
rightPose = rp.getCurrentPose(MyConstants.Arm.Right)
leftGrasp = rp.getCurrentGrasp(MyConstants.Arm.Left)
rightGrasp = rp.getCurrentGrasp(MyConstants.Arm.Right)
leftJoints = invArmKin(MyConstants.Arm.Left, leftPose, leftGrasp)
rightJoints = invArmKin(MyConstants.Arm.Right, rightPose, rightGrasp)
rp.updateOpenraveJoints(MyConstants.Arm.Left, joints1=leftJoints, grasp=leftGrasp)
rp.updateOpenraveJoints(MyConstants.Arm.Right, joints1=rightJoints, grasp=rightGrasp)
raveLeftMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, MyConstants.Frames.Link0)
raveRightMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, MyConstants.Frames.Link0)
raveLeftPose = tfx.pose(raveLeftMatrix,frame=leftPose.frame)
raveRightPose = tfx.pose(raveRightMatrix,frame=rightPose.frame)
deltaLeft = Util.deltaPose(leftPose, raveLeftPose)
deltaRight = Util.deltaPose(rightPose, raveRightPose)
g = []
#g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, 'world'))
#g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, 'world'))
g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, leftPose.matrix, leftPose.frame[1:], 'world'))
g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, rightPose.matrix, rightPose.frame[1:], 'world'))
IPython.embed()
def main():
import roslib
roslib.load_manifest('RavenDebridement')
from RavenDebridement.srv import InvKinSrv
import rospy
from RavenDebridement.RavenCommand.RavenArm import RavenArm
import IPython
rospy.init_node('testKinematics', anonymous=True)
rospy.loginfo('Waiting for IK server...')
rospy.wait_for_service('inv_kin_server', timeout=5)
inv_kin_service = rospy.ServiceProxy('inv_kin_server', InvKinSrv)
from std_msgs.msg import Header
header = Header()
header.frame_id = '/0_link'
header.stamp = rospy.Time.now()
min_limits = np.array([
SHOULDER_MIN_LIMIT, ELBOW_MIN_LIMIT, Z_INS_MIN_LIMIT,
TOOL_ROLL_MIN_LIMIT, TOOL_WRIST_MIN_LIMIT, TOOL_GRASP1_MIN_LIMIT,
TOOL_GRASP2_MIN_LIMIT
])
max_limits = np.array([
SHOULDER_MAX_LIMIT, ELBOW_MAX_LIMIT, Z_INS_MAX_LIMIT,
TOOL_ROLL_MAX_LIMIT, TOOL_WRIST_MAX_LIMIT, TOOL_GRASP1_MAX_LIMIT,
TOOL_GRASP2_MAX_LIMIT
])
armName = GOLD_ARM_ID
for i in xrange(1):
r = np.random.rand(len(min_limits))
val = min_limits + r * (max_limits - min_limits)
joints1 = dict(zip([0, 1, 2, 4, 5, 6, 7], val.tolist()))
joints1[TOOL_ROT] = fix_angle(joints1[TOOL_ROT])
pose, grasp = fwdArmKin(armName, joints1)
joints2 = invArmKin(armName, pose, grasp)
joints2[TOOL_ROT] = fix_angle(joints2[TOOL_ROT])
try:
srvArmName = 0 if armName == GOLD_ARM_ID else 1
resp = inv_kin_service(header, srvArmName, grasp, pose.msg.Pose())
joints3 = dict(
(joint.type, joint.position) for joint in resp.joints)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.loginfo("IK server failure: %s" % e)
# print '\t'.join("%.3f" % v for v in joints1.values())
# print '\t'.join("%.3f" % v for v in joints2.values())
# print '\t'.join("%.3f" % v for v in joints3.values())
# print np.array(joints1.values()) - np.array(joints2.values())
def invArmKinSrv(armId, pose, grasp):
srvArmName = 0 if armId == 'L' else 1
resp = inv_kin_service(header, srvArmName, grasp, pose.msg.Pose())
resp_joints = dict((joint.type, joint.position)
for joint in resp.joints if joint.type <= 7)
return resp_joints
print '-----------------------------------------------'
jl, pl, gl, jr, pr, gr = test_config4()
"""
la = RavenArm('L')
#ra = RavenArm('R')
la.start()
#ra.start()
rospy.sleep(2)
la.stop()
#ra.stop()
print '\n------p->j------'
print '--L--'
jl1 = invArmKin('L', pl, gl)
jl2 = invArmKinSrv('L', pl, gl)
print 'actual:', joint_str(jointRad2deg(jl))
print 'calc: ', joint_str(jointRad2deg(jl1))
print 'calc2: ', joint_str(jointRad2deg(jl2))
"""
print '\n------j->p------'
print '--L--'
pl1, gl1 = fwdArmKin('L', jl)
print 'actual:', pl
print 'calc: ', pl1
print 'actual:', gl
print 'calc: ', gl1
print '--R--'
pr1, gr1 = fwdArmKin('R', jr)
print 'actual:', pr
print 'calc: ', pr1
print 'actual:', gr
print 'calc: ', gr1
print '\n------p->j------'
print '--L--'
jl1 = invArmKin('L', pl, gl)
jl2 = invArmKinSrv('L', pl, gl)
print 'actual:', joint_str(jl)
print 'calc: ', joint_str(jl1)
print 'calc2: ', joint_str(jl2)
print '--R--'
jr1 = invArmKin('R', pr, gr)
jr2 = invArmKinSrv('R', pr, gr)
print 'actual:', joint_str(jr)
print 'calc: ', joint_str(jr1)
print 'calc2: ', joint_str(jr2)
print '\n------j->p->j------'
print '--L--'
jl1 = invArmKin('L', pl1, gl1)
print 'actual:', joint_str(jl)
print 'calc: ', joint_str(jl1)
print '--R--'
jr1 = invArmKin('R', pr1, gr1)
print 'actual:', joint_str(jr)
print 'calc: ', joint_str(jr1)
print '\n------p->j->p------'
print '--L--'
pl1, gl1 = fwdArmKin('L', jl1)
print 'actual:', pl
print 'calc: ', pl1
print 'actual:', gl
print 'calc: ', gl1
print '--R--'
pr1, gr1 = fwdArmKin('R', jr1)
print 'actual:', pr
print 'calc: ', pr1
print 'actual:', gr
print 'calc: ', gr1
from RavenDebridement.Utils import Util
d = Util.deltaPose(pl, pl1)
a = tfx.tb_angles(d.orientation)
IPython.embed()
