Hot-keys on this page

r m x p   toggle line displays

j k   next/prev highlighted chunk

0   (zero) top of page

1   (one) first highlighted chunk

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

''' 

PlanningUtils is a centralised helper to provide functions commonly used by  

motion planners 

''' 

import numpy as np 

from director import robotstate 

 

class PlanningUtils(object): 

    def __init__(self, robotStateModel, robotStateJointController): 

        self.robotStateModel = robotStateModel 

        self.robotStateJointController = robotStateJointController 

 

        # Get joint limits 

        self.jointLimitsLower = np.array([self.robotStateModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames()]) 

        self.jointLimitsUpper = np.array([self.robotStateModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames()]) 

 

        # Activate/Deactivate joint limit clamper 

        self.clampToJointLimits = False 

 

    def getPlanningStartPose(self): 

        startPose = np.array(self.robotStateJointController.q) 

 

        if self.clampToJointLimits: 

            startPose = np.clip(startPose, self.jointLimitsLower, self.jointLimitsUpper) 

 

        return startPose