def planReach(self, reachTargetFrame=None, jointSpeedLimit=None):

        if reachTargetFrame is None:
            reachTargetFrame = self.doorHandleReachFrame

        if jointSpeedLimit is None:
            jointSpeedLimit = self.speedLow

        startPose = self.getPlanningStartPose()
        constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, reachTargetFrame)
        constraintSet.nominalPoseName = 'q_start'
        constraintSet.ikParameters =  IkParameters(usePointwise=False,
                                                   maxDegreesPerSecond=self.speedLow,
                                                   numberOfAddedKnots=2)

        endPose, info = constraintSet.runIk()

        linkOffsetFrame = self.ikPlanner.getPalmToHandLink(self.graspingHand)
        handLinkName = self.ikPlanner.getHandLink(self.graspingHand)
        handToWorld1 = self.ikPlanner.getLinkFrameAtPose(handLinkName, startPose)
        handToWorld2 = self.ikPlanner.getLinkFrameAtPose(handLinkName, endPose)
        palmToWorld1 = transformUtils.concatenateTransforms([linkOffsetFrame, handToWorld1])
        palmToWorld2 = transformUtils.concatenateTransforms([linkOffsetFrame, handToWorld2])

        motionVector = np.array(palmToWorld1.GetPosition()) - np.array(palmToWorld2.GetPosition())
        motionTargetFrame = transformUtils.getTransformFromOriginAndNormal(np.array(palmToWorld2.GetPosition()), motionVector)

        p = self.ikPlanner.createLinePositionConstraint(handLinkName, linkOffsetFrame, motionTargetFrame, lineAxis=2, bounds=[-np.linalg.norm(motionVector), 0.0], positionTolerance=0.001)
        constraintSet.constraints.append(p)

        plan = constraintSet.runIkTraj()
        self.addPlan(plan)
dataset/ETHPy150Open RobotLocomotion/director/src/python/director/doordemo.py/DoorDemo.planReach/swapped operands of `-`