def planLeftFootDown(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_footdown_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_footdown_end'
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, 0.0)
constraints = []
constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0]))
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True,
pelvisEnabled=False, shrinkFactor=0.01))
constraints.append(self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint())
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
#constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot'))
constraints.extend(self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,1]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True)
#constraintSet.seedPoseName = 'q_start'
#constraintSet.nominalPoseName = 'q_start'
endPose, _ = constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0], poseTimes[-1]]
supportsList = [['r_foot'], ['r_foot','l_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True)
self.addPlan(plan)
return plan, endPose
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 runIK(self,
targetFrame,
startPose=None,
graspToHandLinkFrame=None,
makePlan=True,
positionTolerance=0.0,
angleToleranceInDegrees=5.0,
maxDegreesPerSecond=60):
"""
Sets the cameraFrame to the targetFrame using IK
:param targetFrame:
:return:
"""
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.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(
maxDegreesPerSecond=maxDegreesPerSecond)
endPose, info = constraintSet.runIk()
returnData = dict()
returnData['info'] = info
returnData['endPose'] = endPose
if makePlan:
plan = constraintSet.planEndPoseGoal()
returnData['plan'] = plan
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
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 planShiftWeightOut(self, startPose=None):
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
constraints = []
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
constraints.extend(self.createUtorsoGazeConstraints([1.0, 1.0]))
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=self.quasiStaticShrinkFactor))
constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'l_foot'))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot'))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False)
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 planStandUp(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
pelvisFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('pelvis', startPose)
t = transformUtils.frameFromPositionAndRPY([self.pelvisLiftX, 0, self.pelvisLiftZ], [0, 0, 0])
liftFrame = transformUtils.concatenateTransforms([t, pelvisFrame])
constraints = []
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
g = self.createUtorsoGazeConstraints([1.0, 1.0])
constraints.append(g[1])
p = ik.PositionConstraint(linkName='pelvis', referenceFrame=liftFrame,
lowerBound=np.array([0.0, -np.inf, 0.0]),
upperBound=np.array([np.inf, np.inf, 0.0]))
constraints.append(p)
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True, pelvisEnabled=False,
shrinkFactor=self.quasiStaticShrinkFactor))
constraints.append(self.robotSystem.ikPlanner.createXYZMovingBasePostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
constraints.extend(self.robotSystem.ikPlanner.createFixedFootConstraints(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseMetersPerSecond=0.02)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=True)
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 planGetWeightOverFeet(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
constraints = []
constraints.append(
ikconstraints.QuasiStaticConstraint(leftFootEnabled=True,
rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=0.5))
constraints.append(
self.robotSystem.ikPlanner.createLockedBasePostureConstraint(
startPoseName))
constraints.append(
self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(
startPoseName))
constraints.append(
self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(
startPoseName))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints,
endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=False,
maxDegreesPerSecond=10)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot', 'l_foot', 'pelvis']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts,
True)
self.addPlan(plan)
return plan
def planFootOut(self, startPose=None, finalFootHeight=0.05):
if startPose is None:
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
utorsoFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('utorso', startPose)
finalLeftFootFrame = self.computeLeftFootOverPlatformFrame(startPose, finalFootHeight)
constraints = []
constraints.extend(self.createUtorsoGazeConstraints([0.0, 1.0]))
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=False, rightFootEnabled=True,
pelvisEnabled=False, shrinkFactor=0.01))
constraints.append(self.robotSystem.ikPlanner.createMovingBaseSafeLimitsConstraint())
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
#constraints.append(self.robotSystem.ikPlanner.createLockedBackPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.robotSystem.ikPlanner.createFixedLinkConstraints(startPoseName, 'r_foot'))
constraints.extend(self.createLeftFootPoseConstraint(finalLeftFootFrame, tspan=[1,1]))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=True, maxBaseRPYDegreesPerSecond=10,
rescaleBodyNames=['l_foot'],
rescaleBodyPts=[0.0, 0.0, 0.0],
maxBodyTranslationSpeed=self.maxFootTranslationSpeed)
#constraintSet.seedPoseName = 'q_start'
#constraintSet.nominalPoseName = 'q_start'
constraintSet.runIk()
footFrame = self.robotSystem.ikPlanner.getLinkFrameAtPose('l_foot', startPose)
t = transformUtils.frameFromPositionAndRPY([0, 0, self.polaris.leftFootEgressOutsideFrame.transform.GetPosition()[2]-footFrame.GetPosition()[2]], [0, 0, 0])
liftFrame = transformUtils.concatenateTransforms([footFrame, t])
vis.updateFrame(liftFrame, 'lift frame')
c = ik.WorldFixedOrientConstraint()
c.linkName = 'l_foot'
c.tspan = [0.0, 0.1, 0.2]
constraints.append(c)
constraints.extend(self.createLeftFootPoseConstraint(liftFrame, tspan=[0.2, 0.2]))
constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressMidFrame, tspan=[0.5, 0.5]))
constraints.extend(self.createLeftFootPoseConstraint(self.polaris.leftFootEgressOutsideFrame, tspan=[0.8, 0.8]))
#plan = constraintSet.planEndPoseGoal(feetOnGround=False)
keyFramePlan = constraintSet.runIkTraj()
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, False)
self.addPlan(plan)
return plan
def planGetWeightOverFeet(self):
startPose = self.getPlanningStartPose()
startPoseName = 'q_egress_start'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
endPoseName = 'q_egress_end'
constraints = []
constraints.append(ik.QuasiStaticConstraint(leftFootEnabled=True, rightFootEnabled=True,
pelvisEnabled=False,
shrinkFactor=0.5))
constraints.append(self.robotSystem.ikPlanner.createLockedBasePostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.robotSystem.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
constraintSet = ConstraintSet(self.robotSystem.ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(usePointwise=False, maxDegreesPerSecond=10)
constraintSet.runIk()
keyFramePlan = constraintSet.planEndPoseGoal(feetOnGround=False)
poseTimes, poses = planplayback.PlanPlayback.getPlanPoses(keyFramePlan)
ts = [poseTimes[0]]
supportsList = [['r_foot', 'l_foot', 'pelvis']]
plan = self.publishPlanWithSupports(keyFramePlan, supportsList, ts, True)
self.addPlan(plan)
return plan
