def planCenterWeight(self, startPose=None):
ikPlanner = self.robotSystem.ikPlanner
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_lean_right'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
footFixedConstraints = ikPlanner.createFixedFootConstraints(startPoseName)
backConstraint = ikPlanner.createMovingBackLimitedPostureConstraint()
armsLocked = ikPlanner.createLockedArmsPostureConstraints(startPoseName)
constraints = [backConstraint]
constraints.extend(footFixedConstraints)
constraints.extend(armsLocked)
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=self.quasiStaticShrinkFactor))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraintSet = ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.seedPoseName = 'q_start'
constraintSet.nominalPoseName = 'q_nom'
endPose = constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal()
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot','l_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True)
self.addPlan(plan)
return plan
def runIK(self,
targetFrame,
startPose=None,
graspToHandLinkFrame=None,
positionTolerance=0.0,
angleToleranceInDegrees=0.0,
seedPoseName='q_nom'):
if graspToHandLinkFrame is None:
graspToHandLinkFrame = vtk.vtkTransform()
if startPose is None:
startPose = self.getPlanningStartPose()
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'reach_start'
endPoseName = 'reach_end'
ikPlanner.addPose(startPose, startPoseName)
side = ikPlanner.reachingSide
constraints = []
constraints.append(
KukaPlanningUtils.createLockedBasePostureConstraint(
ikPlanner, startPoseName))
# positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame,
# graspToHandLinkFrame,
# positionTolerance=positionTolerance,
# angleToleranceInDegrees=angleToleranceInDegrees)
positionConstraint, orientationConstraint = ikPlanner.createPositionOrientationConstraint(
self.endEffectorLinkName,
targetFrame,
graspToHandLinkFrame,
positionTolerance=positionTolerance,
angleToleranceInDegrees=angleToleranceInDegrees)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ConstraintSet(ikPlanner, constraints, 'reach_end',
startPoseName)
constraintSet.ikParameters = IkParameters()
constraintSet.seedPoseName = seedPoseName
print "consraintSet ", constraintSet
print "constraintSet.seedPoseName ", constraintSet.seedPoseName
print "constraintSet.nominalPoseName ", constraintSet.nominalPoseName
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], 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
