def planInsertTraj(self, speed, lockFeet=True, lockBase=None, resetBase=False, wristAngleCW=0,
startPose=None, verticalOffset=0.01, usePoses=False, resetPoses=True,
planFromCurrentRobotState=False, retract=False):
ikParameters = IkParameters(usePointwise=False, maxDegreesPerSecond=speed,
numberOfAddedKnots=1,
quasiStaticShrinkFactor=self.quasiStaticShrinkFactor,
fixInitialState=planFromCurrentRobotState)
_, yaxis, _ = transformUtils.getAxesFromTransform(self.computeGraspFrame().transform)
yawDesired = np.arctan2(yaxis[1], yaxis[0])
if startPose is None:
startPose = self.getPlanningStartPose()
nominalPose = self.getNominalPose()
self.ikPlanner.addPose(nominalPose, self.nominalPoseName)
self.ikPlanner.addPose(startPose, self.startPoseName)
self.ikPlanner.reachingSide = self.graspingHand
constraints = []
constraints.extend(self.createBaseConstraints(resetBase, lockBase, lockFeet, yawDesired))
constraints.append(self.createBackPostureConstraint())
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.extend(self.createFootConstraints(lockFeet))
constraints.append(self.ikPlanner.createLockedArmPostureConstraint(self.startPoseName))
constraints.append(self.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.createElbowPostureConstraint())
constraints.append(self.createStaticTorqueConstraint())
constraints.append(self.createHandGazeConstraint())
constraints.append(self.createHandFixedOrientConstraint())
constraints.append(self.createWristAngleConstraint(wristAngleCW,
planFromCurrentRobotState))
constraints.extend(self.createAllHandPositionConstraints(self.coaxialTol, retract))
if retract:
startPoseName = self.getStartPoseName(planFromCurrentRobotState, True, usePoses)
endPoseName = self.getEndPoseName(True, usePoses)
endPose = self.ikPlanner.jointController.getPose(endPoseName)
endPose = self.ikPlanner.mergePostures(endPose, robotstate.matchJoints('lwy'), startPose)
endPoseName = 'q_retract'
self.ikPlanner.addPose(endPose, endPoseName)
else:
startPoseName = self.getStartPoseName(planFromCurrentRobotState, retract, usePoses)
endPoseName = self.getEndPoseName(retract, usePoses)
plan = self.ikPlanner.runIkTraj(constraints, startPoseName, endPoseName, self.nominalPoseName, ikParameters=ikParameters)
if resetPoses and not retract and max(plan.plan_info) <= 10:
self.setReachAndTouchPoses(plan)
return plan
def createAllButLeftLegPostureConstraint(self, poseName):
joints = robotstate.matchJoints('^(?!l_leg)')
return self.robotSystem.ikPlanner.createPostureConstraint(poseName, joints)
def planInsertTraj(self,
speed,
lockFeet=True,
lockBase=None,
resetBase=False,
wristAngleCW=0,
startPose=None,
verticalOffset=0.01,
usePoses=False,
resetPoses=True,
planFromCurrentRobotState=False,
retract=False):
ikParameters = IkParameters(
usePointwise=False,
maxDegreesPerSecond=speed,
numberOfAddedKnots=1,
quasiStaticShrinkFactor=self.quasiStaticShrinkFactor,
fixInitialState=planFromCurrentRobotState)
_, yaxis, _ = transformUtils.getAxesFromTransform(
self.computeGraspFrame().transform)
yawDesired = np.arctan2(yaxis[1], yaxis[0])
if startPose is None:
startPose = self.getPlanningStartPose()
nominalPose = self.getNominalPose()
self.ikPlanner.addPose(nominalPose, self.nominalPoseName)
self.ikPlanner.addPose(startPose, self.startPoseName)
self.ikPlanner.reachingSide = self.graspingHand
constraints = []
constraints.extend(
self.createBaseConstraints(resetBase, lockBase, lockFeet,
yawDesired))
constraints.append(self.createBackPostureConstraint())
constraints.append(self.ikPlanner.createQuasiStaticConstraint())
constraints.extend(self.createFootConstraints(lockFeet))
constraints.append(
self.ikPlanner.createLockedArmPostureConstraint(
self.startPoseName))
constraints.append(
self.ikPlanner.createKneePostureConstraint([0.7, 2.5]))
constraints.append(self.createElbowPostureConstraint())
constraints.append(self.createStaticTorqueConstraint())
constraints.append(self.createHandGazeConstraint())
constraints.append(self.createHandFixedOrientConstraint())
constraints.append(
self.createWristAngleConstraint(wristAngleCW,
planFromCurrentRobotState))
constraints.extend(
self.createAllHandPositionConstraints(self.coaxialTol, retract))
if retract:
startPoseName = self.getStartPoseName(planFromCurrentRobotState,
True, usePoses)
endPoseName = self.getEndPoseName(True, usePoses)
endPose = self.ikPlanner.jointController.getPose(endPoseName)
endPose = self.ikPlanner.mergePostures(
endPose, robotstate.matchJoints('lwy'), startPose)
endPoseName = 'q_retract'
self.ikPlanner.addPose(endPose, endPoseName)
else:
startPoseName = self.getStartPoseName(planFromCurrentRobotState,
retract, usePoses)
endPoseName = self.getEndPoseName(retract, usePoses)
plan = self.ikPlanner.runIkTraj(constraints,
startPoseName,
endPoseName,
self.nominalPoseName,
ikParameters=ikParameters)
if resetPoses and not retract and max(plan.plan_info) <= 10:
self.setReachAndTouchPoses(plan)
return plan
