def createBrokenArmConstraint(self):
p = ik.PostureConstraint()
p.joints = ['l_arm_elx']
p.jointsLowerBound = [0.673677]
p.jointsUpperBound = [np.inf]
p.tspan = [1, 1]
return p
def createElbowPostureConstraint(self):
if self.graspingHand == 'left':
elxJoint = 'l_arm_elx'
elxLowerBound = np.radians(self.elxLowerBoundDegrees)
elxUpperBound = 2.5
else:
elxJoint = 'r_arm_elx'
elxLowerBound = -2.5
elxUpperBound = np.radians(-self.elxLowerBoundDegrees)
constraint = ik.PostureConstraint()
constraint.joints = [elxJoint]
constraint.jointsLowerBound = [elxLowerBound]
constraint.jointsUpperBound = [elxUpperBound]
return constraint
def createWristAngleConstraint(self, wristAngleCW,
planFromCurrentRobotState):
if self.graspingHand == 'left':
wristJoint = ['l_arm_lwy']
wristJointLowerBound = [-np.radians(160) + wristAngleCW]
wristJointUpperBound = [-np.radians(160) + wristAngleCW]
else:
wristJoint = ['r_arm_lwy']
wristJointLowerBound = [np.radians(160) - wristAngleCW]
wristJointUpperBound = [np.radians(160) - wristAngleCW]
constraint = ik.PostureConstraint()
constraint.joints = wristJoint
constraint.jointsLowerBound = wristJointLowerBound
constraint.jointsUpperBound = wristJointUpperBound
if planFromCurrentRobotState:
constraint.tspan = [1.0, 1.0]
return constraint
def createBaseConstraints(self, resetBase, lockBase, lockFeet, yawDesired):
constraints = []
if lockBase is None:
lockBase = self.lockBase
if resetBase:
poseName = self.nominalPoseName
else:
poseName = self.startPoseName
if lockFeet:
if lockBase:
constraints.append(
self.ikPlanner.createLockedBasePostureConstraint(
poseName, lockLegs=False))
else:
constraints.append(
self.ikPlanner.createXYZMovingBasePostureConstraint(
poseName))
constraints.append(
ik.WorldFixedBodyPoseConstraint(linkName='pelvis'))
else:
constraints.append(
self.ikPlanner.createXYZYawMovingBasePostureConstraint(
poseName))
constraints.append(
ik.WorldFixedBodyPoseConstraint(linkName='pelvis'))
constraints.append(self.createHeadGazeConstraint())
p = ik.PostureConstraint()
p.joints = ['base_yaw']
p.jointsLowerBound = [yawDesired - np.radians(20)]
p.jointsUpperBound = [yawDesired + np.radians(20)]
constraints.append(p)
return constraints