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 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 computeIk(goalFrame, constraints, ikParameters, seedPoseName, nominalPoseName):
constraints[-2].referenceFrame = goalFrame
constraints[-1].quaternion = goalFrame
cs = ikplanner.ConstraintSet(robotSystem.ikPlanner, constraints, '', '')
cs.seedPoseName = seedPoseName
cs.nominalPoseName = nominalPoseName
return cs.runIk()
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])
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 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 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 updateIKConstraints(self):
startPoseName = 'reach_start'
startPose = np.array(self.robotStateJointController.q)
self.ikPlanner.addPose(startPose, startPoseName)
constraints = []
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.append(
self.ikPlanner.createLockedNeckPostureConstraint(startPoseName))
# Get base constraint
if self.getComboText(self.ui.baseComboBox) == 'Fixed':
constraints.append(
self.ikPlanner.createLockedBasePostureConstraint(
startPoseName, lockLegs=False))
self.ikPlanner.setBaseLocked(True)
elif self.getComboText(self.ui.baseComboBox) == 'XYZ only':
constraints.append(
self.ikPlanner.createXYZMovingBasePostureConstraint(
startPoseName))
constraints.append(
self.ikPlanner.createKneePostureConstraint(
self.kneeJointLimits))
elif self.getComboText(self.ui.baseComboBox) == 'Limited':
constraints.append(
self.ikPlanner.createMovingBaseSafeLimitsConstraint())
constraints.append(
self.ikPlanner.createKneePostureConstraint(
self.kneeJointLimits))
self.ikPlanner.setBaseLocked(False)
# Get back constraint
if self.getComboText(self.ui.backComboBox) == 'Fixed':
constraints.append(
self.ikPlanner.createLockedBackPostureConstraint(
startPoseName))
self.ikPlanner.setBackLocked(True)
elif self.getComboText(self.ui.backComboBox) == 'Limited':
constraints.append(
self.ikPlanner.createMovingBackLimitedPostureConstraint())
self.ikPlanner.setBackLocked(False)
# Get feet constraint
if self.getComboText(self.ui.feetComboBox) == 'Fixed':
constraints.append(
self.ikPlanner.createFixedLinkConstraints(
startPoseName,
self.ikPlanner.leftFootLink,
tspan=[0.0, 1.0],
lowerBound=-0.0001 * np.ones(3),
upperBound=0.0001 * np.ones(3),
angleToleranceInDegrees=0.1))
constraints.append(
self.ikPlanner.createFixedLinkConstraints(
startPoseName,
self.ikPlanner.rightFootLink,
tspan=[0.0, 1.0],
lowerBound=-0.0001 * np.ones(3),
upperBound=0.0001 * np.ones(3),
angleToleranceInDegrees=0.1))
elif self.getComboText(self.ui.feetComboBox) == 'Sliding':
constraints.extend(
self.ikPlanner.createSlidingFootConstraints(startPoseName)[:2])
constraints.extend(
self.ikPlanner.createSlidingFootConstraints(startPoseName)[2:])
# Ensure the end-pose's relative distance between two feet is the same as start pose
[pos_left, quat_left] = transformUtils.poseFromTransform(
self.robotStateModel.getLinkFrame(self.ikPlanner.leftFootLink))
[pos_right, quat_right] = transformUtils.poseFromTransform(
self.robotStateModel.getLinkFrame(
self.ikPlanner.rightFootLink))
dist = npla.norm(pos_left - pos_right)
constraints.append(
ik.PointToPointDistanceConstraint(
bodyNameA=self.ikPlanner.leftFootLink,
bodyNameB=self.ikPlanner.rightFootLink,
lowerBound=np.array([dist - 0.0001]),
upperBound=np.array([dist + 0.0001])))
sides = []
if self.getReachHand() == 'Left':
sides.append('left')
elif self.getReachHand() == 'Right':
sides.append('right')
elif self.getReachHand() == 'Both':
sides.append('left')
sides.append('right')
if self.getComboText(self.ui.otherHandComboBox) == 'Fixed':
if not 'left' in sides:
self.ikPlanner.setArmLocked('left', True)
constraints.append(
self.ikPlanner.createLockedLeftArmPostureConstraint(
startPoseName))
if not 'right' in sides:
self.ikPlanner.setArmLocked('right', True)
constraints.append(
self.ikPlanner.createLockedRightArmPostureConstraint(
startPoseName))
for side in sides:
linkName = self.ikPlanner.getHandLink(side)
graspToHand = self.ikPlanner.newPalmOffsetGraspToHandFrame(
side, self.palmOffsetDistance)
graspToWorld = self.getGoalFrame(linkName)
p, q = self.ikPlanner.createPositionOrientationGraspConstraints(
side, graspToWorld, graspToHand)
p.tspan = [1.0, 1.0]
q.tspan = [1.0, 1.0]
constraints.extend([p, q])
constraints.append(
self.ikPlanner.createActiveEndEffectorConstraint(
linkName, self.ikPlanner.getPalmPoint(side)))
self.constraintSet = ikplanner.ConstraintSet(
self.ikPlanner, constraints, 'reach_end', startPoseName)