def computeSingleCameraPose(self, targetLocationWorld=[1,0,0], cameraFrameLocation=[0.22, 0, 0.89], flip=False):
cameraAxis = [0,0,1]
linkName = self.handFrame
linkName = 'iiwa_link_7'
linkFrame =self.robotSystem.robotStateModel.getLinkFrame(linkName)
cameraFrame = self.robotSystem.robotStateModel.getLinkFrame(self.handFrame)
cameraToLinkFrame = transformUtils.concatenateTransforms([cameraFrame, linkFrame.GetLinearInverse()])
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'q_nom'
endPoseName = 'reach_end'
seedPoseName = 'q_nom'
if flip:
print "FLIPPING startPoseName"
startPoseName = 'q_nom_invert_7th_joint'
seedPoseName = 'q_nom_invert_7th_joint'
pose = np.asarray([ 0. , 0. , 0. , 0. , 0. , 0. , 0. ,
-0.68 , 1.0 , -1.688 , 1.0 , -0.5635, 1.0 ])
ikPlanner.addPose(pose, startPoseName)
else:
print "NOT flipped"
constraints = []
constraints.append(ikPlanner.createPostureConstraint(startPoseName, robotstate.matchJoints('base_')))
positionConstraint = HandEyeCalibration.createPositionConstraint(targetPosition=cameraFrameLocation, linkName=linkName, linkOffsetFrame=cameraToLinkFrame, positionTolerance=0.05)
cameraGazeConstraint = HandEyeCalibration.createCameraGazeTargetConstraint(linkName=linkName, cameraToLinkFrame=cameraToLinkFrame, cameraAxis=cameraAxis, worldPoint=targetLocationWorld, coneThresholdDegrees=5.0)
constraints.append(positionConstraint)
constraints.append(cameraGazeConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
if flip:
constraintSet.ikPlanner.addPose(pose, startPoseName)
constraintSet.ikParameters = IkParameters()
constraintSet.seedPoseName = seedPoseName
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
return returnData
def computeSingleCameraPose(self,
targetLocationWorld=[1, 0, 0],
cameraFrameLocation=[0.22, 0, 0.89]):
cameraAxis = [0, -1,
0] # assuming we are using 'palm' as the link frame
linkName = self.handFrame
linkName = 'iiwa_link_7'
linkFrame = self.robotSystem.robotStateModel.getLinkFrame(linkName)
cameraFrame = self.robotSystem.robotStateModel.getLinkFrame(
self.handFrame)
cameraToLinkFrame = transformUtils.concatenateTransforms(
[cameraFrame, linkFrame.GetLinearInverse()])
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'q_nom'
endPoseName = 'reach_end'
seedPoseName = 'q_nom'
constraints = []
constraints.append(
ikPlanner.createPostureConstraint(startPoseName,
robotstate.matchJoints('base_')))
positionConstraint = HandEyeCalibration.createPositionConstraint(
targetPosition=cameraFrameLocation,
linkName=linkName,
linkOffsetFrame=cameraToLinkFrame,
positionTolerance=0.05)
cameraGazeConstraint = HandEyeCalibration.createCameraGazeTargetConstraint(
linkName=linkName,
cameraToLinkFrame=cameraToLinkFrame,
cameraAxis=cameraAxis,
worldPoint=targetLocationWorld,
coneThresholdDegrees=5.0)
constraints.append(positionConstraint)
constraints.append(cameraGazeConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
constraintSet.ikParameters = IkParameters()
constraintSet.seedPoseName = seedPoseName
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
return returnData
def planReachGoal(goalFrameName='reach goal',
startPose=None,
planTraj=True,
interactive=False,
seedFromStart=False):
goalFrame = om.findObjectByName(goalFrameName).transform
startPoseName = 'reach_start'
endPoseName = 'reach_end'
endEffectorLinkName = getEndEffectorLinkName()
graspOffsetFrame = getGraspToHandLink()
if startPose is None:
startPose = robotSystem.planningUtils.getPlanningStartPose()
ikPlanner.addPose(startPose, startPoseName)
constraints = []
constraints.append(
ikPlanner.createPostureConstraint(startPoseName,
robotstate.matchJoints('base_')))
p, q = ikPlanner.createPositionOrientationConstraint(
endEffectorLinkName,
goalFrame,
graspOffsetFrame,
positionTolerance=0.0,
angleToleranceInDegrees=0.0)
p.tspan = [1.0, 1.0]
q.tspan = [1.0, 1.0]
_, _, axisConstraint = ikPlanner.createMoveOnLineConstraints(
startPose, goalFrame, graspOffsetFrame)
axisConstraint.tspan = np.linspace(0, 1, 10)
isPregrasp = goalFrameName.startswith('pregrasp to world')
isGrasp = goalFrameName.startswith('grasp to world')
if isGrasp:
seedFromStart = True
# adjust bounds of move on line constraint
axisConstraintTubeRadius = 0.3 if isPregrasp else 0.001
axisConstraint.lowerBound[0] = -axisConstraintTubeRadius
axisConstraint.lowerBound[0] = -axisConstraintTubeRadius
axisConstraint.upperBound[1] = axisConstraintTubeRadius
axisConstraint.upperBound[1] = axisConstraintTubeRadius
# allow sliding in Z axis of pinch frame
# this may be overruled by the line constraint
if isPregrasp:
p.lowerBound[2] = -0.02
p.upperBound[2] = 0.02
# align the gripper pinch axis
# with the Y axis of the goal frame
g = ikconstraints.WorldGazeDirConstraint()
g.linkName = endEffectorLinkName
g.targetFrame = goalFrame
g.targetAxis = [0, 1, 0]
g.bodyAxis = list(graspOffsetFrame.TransformVector([0, 1, 0]))
g.coneThreshold = np.radians(5.0) if isPregrasp else np.radians(0.0)
g.tspan = [1.0, 1.0]
# point the fingers along the X axis
# of the goal frame
pinchPivotBound = np.radians(20) if isPregrasp else np.radians(0)
g2 = ikconstraints.WorldGazeDirConstraint()
g2.linkName = endEffectorLinkName
g2.targetFrame = goalFrame
g2.targetAxis = [1, 0, 0]
g2.bodyAxis = list(graspOffsetFrame.TransformVector([1, 0, 0]))
g2.coneThreshold = pinchPivotBound
g2.tspan = [1.0, 1.0]
constraints.append(p)
constraints.append(q)
#constraints.append(g)
#constraints.append(g2)
constraints.append(axisConstraint)
constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints,
endPoseName, startPoseName)
constraintSet.ikParameters.usePointwise = True
if seedFromStart:
constraintSet.seedPoseName = startPoseName
constraintSet.nominalPoseName = startPoseName
elif isPregrasp:
constraintSet.seedPoseName = 'q_nom'
constraintSet.nominalPoseName = 'q_nom'
global _callbackId
#if _callbackId:
# om.findObjectByName(goalFrameName).disconnectFrameModified(_callbackId)
if interactive:
def update(frame):
endPose, info = constraintSet.runIk()
robotSystem.teleopPanel.showPose(endPose)
_callbackId = om.findObjectByName(goalFrameName).connectFrameModified(
update)
update(None)
else:
endPose, info = constraintSet.runIk()
if planTraj:
robotSystem.teleopPanel.hideTeleopModel()
return constraintSet.runIkTraj()
else:
return endPose, info
def createAllButLeftLegPostureConstraint(self, poseName):
joints = robotstate.matchJoints('^(?!l_leg)')
return self.robotSystem.ikPlanner.createPostureConstraint(poseName, joints)
def createLockedBasePostureConstraint(ikPlanner, startPoseName):
return ikPlanner.createPostureConstraint(
startPoseName, robotstate.matchJoints('base_'))
