def initGazeConstraintSet(self, goalFrame):
# create constraint set
startPose = robotSystem.robotStateJointController.q.copy()
startPoseName = 'gaze_plan_start'
endPoseName = 'gaze_plan_end'
self.ikPlanner.addPose(startPose, startPoseName)
self.ikPlanner.addPose(startPose, endPoseName)
self.constraintSet = ikplanner.ConstraintSet(self.ikPlanner, [],
startPoseName,
endPoseName)
self.constraintSet.endPose = startPose
# add body constraints
bodyConstraints = self.ikPlanner.createMovingBodyConstraints(
startPoseName,
lockBase=True,
lockBack=False,
lockLeftArm=self.graspingHand == 'right',
lockRightArm=self.graspingHand == 'left')
self.constraintSet.constraints.extend(bodyConstraints)
# add gaze constraint
self.graspToHandLinkFrame = self.ikPlanner.newPalmOffsetGraspToHandFrame(
self.graspingHand, self.properties.getProperty('Palm offset'))
gazeConstraint = self.ikPlanner.createGazeGraspConstraint(
self.graspingHand,
goalFrame,
self.graspToHandLinkFrame,
coneThresholdDegrees=self.properties.getProperty(
'Cone threshold degrees'))
self.constraintSet.constraints.insert(0, gazeConstraint)
def planReach(self):
ikPlanner = self.robotSystem.ikPlanner
startPose = self.getPlanningStartPose()
startPoseName = 'q_reach_start'
endPoseName = 'q_reach_end'
self.robotSystem.ikPlanner.addPose(startPose, startPoseName)
side = 'right'
movingReachConstraint = ikPlanner.createMovingReachConstraints(startPoseName, lockBase=True, lockBack=True, lockArm=True, side=side)
palmToHand = ikPlanner.getPalmToHandLink(side=side)
targetFrame = om.findObjectByName('reach frame').transform
poseConstraints = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame, graspToHandLinkFrame=palmToHand, angleToleranceInDegrees=5.0)
constraints = []
constraints.extend(movingReachConstraint)
constraints.extend(poseConstraints)
constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName)
constraintSet.ikParameters = IkParameters(maxDegreesPerSecond=30)
seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right')
seedPoseName = 'q_above_switch'
self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName)
constraintSet.seedPoseName = seedPoseName
constraintSet.nominalPoseName = seedPoseName
endPose, info = constraintSet.runIk()
plan = constraintSet.planEndPoseGoal()
self.addPlan(plan)
def initGazeConstraintSet(self,
goalFrame,
gazeHand,
gazeToHandLinkFrame,
targetAxis=[-1.0, 0.0, 0.0],
bodyAxis=[-1.0, 0.0, 0.0],
lockBase=False,
lockBack=False):
# create constraint set
startPose = self.getPlanningStartPose()
startPoseName = 'gaze_plan_start'
endPoseName = 'gaze_plan_end'
self.ikPlanner.addPose(startPose, startPoseName)
self.ikPlanner.addPose(startPose, endPoseName)
self.constraintSet = ikplanner.ConstraintSet(self.ikPlanner, [],
startPoseName,
endPoseName)
self.constraintSet.endPose = startPose
# add body constraints
bodyConstraints = self.ikPlanner.createMovingBodyConstraints(
startPoseName,
lockBase=lockBase,
lockBack=lockBack,
lockLeftArm=gazeHand == 'right',
lockRightArm=gazeHand == 'left')
self.constraintSet.constraints.extend(bodyConstraints)
coneThresholdDegrees = 0.0
gazeConstraint = self.ikPlanner.createGazeGraspConstraint(
gazeHand, goalFrame, gazeToHandLinkFrame, coneThresholdDegrees,
targetAxis, bodyAxis)
self.constraintSet.constraints.insert(0, gazeConstraint)
def getNextDoubleSupportPose(self, lfootTransform, rfootTransform):
vis.updateFrame(lfootTransform, 'lfootTransform', visible=True, scale=0.2)
vis.updateFrame(rfootTransform, 'rfootTransform', visible=True, scale=0.2)
startPose = self.robotStateJointController.getPose('EST_ROBOT_STATE')
startPoseName = 'stride_start'
self.ikPlanner.addPose(startPose, startPoseName)
constraints = []
# lock everything except the feet, constrain the feet
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.append(self.ikPlanner.createMovingBackPostureConstraint())
constraints.append(self.ikPlanner.createMovingBasePostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedLeftArmPostureConstraint(startPoseName))
constraints.append(self.ikPlanner.createLockedRightArmPostureConstraint(startPoseName))
nullFrame = vtk.vtkTransform()
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.rightFootLink, rfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
positionConstraint, orientationConstraint = self.ikPlanner.createPositionOrientationConstraint(self.ikPlanner.leftFootLink, lfootTransform, nullFrame)
positionConstraint.tspan = [1.0, 1.0]
orientationConstraint.tspan = [1.0, 1.0]
constraints.append(positionConstraint)
constraints.append(orientationConstraint)
constraintSet = ikplanner.ConstraintSet(self.ikPlanner, constraints, 'stride_end', startPoseName)
nextDoubleSupportPose, info = constraintSet.runIk()
return nextDoubleSupportPose
def initConstraintSet(self):
# create constraint set
startPose = self.getPlanningStartPose()
startPoseName = 'gaze_plan_start'
endPoseName = 'gaze_plan_end'
self.ikPlanner.addPose(startPose, startPoseName)
self.ikPlanner.addPose(startPose, endPoseName)
self.constraintSet = ikplanner.ConstraintSet(self.ikPlanner, [], startPoseName, endPoseName)
self.constraintSet.endPose = startPose
# add body constraints
bodyConstraints = self.ikPlanner.createMovingBodyConstraints(startPoseName, lockBase=self.lockBase, lockBack=self.lockBack, lockLeftArm=self.graspingHand=='right', lockRightArm=self.graspingHand=='left')
self.constraintSet.constraints.extend(bodyConstraints)
def planWeightShift(self):
ikPlanner = self.robotSystem.ikPlanner
startPoseName = 'plan_start'
endPoseName = 'plan_end'
startPose = self.robotSystem.robotStateJointController.q
ikPlanner.addPose(startPose, startPoseName)
constraints = ikPlanner.createMovingBodyConstraints(startPoseName,
lockBack=True,
lockLeftArm=True,
lockRightArm=True)
constraints[0].rightFootEnabled = False
constraints[0].shrinkFactor = 0.1
constraints.append(ikPlanner.createKneePostureConstraint([1, 2.5]))
cs = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName,
startPoseName)
endPose, info = cs.runIk()
ikPlanner.computeMultiPostureGoal([startPose, endPose])