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 = ikconstraints.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(
ikconstraints.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 createPositionConstraint(targetPosition=None, positionTolerance=0.0, linkName=None, linkOffsetFrame=None):
if linkOffsetFrame is None:
linkOffsetFrame = vtk.vtkTransform()
p = ikconstraints.PositionConstraint()
p.linkName = linkName
p.pointInLink = np.array(linkOffsetFrame.GetPosition())
targetFrame = None
if isinstance(targetPosition, vtk.vtkTransform):
targetFrame = targetPosition
else:
quat = [1,0,0,0]
targetFrame = transformUtils.transformFromPose(targetPosition, quat)
p.referenceFrame = targetFrame
p.lowerBound = np.tile(-positionTolerance, 3)
p.upperBound = np.tile(positionTolerance, 3)
return p