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

27

28

29

30

31

32

33

34

35

36

37

38

39

40

import director.vtkAll as vtk 

import director.thirdparty.numpyjsoncoder as nje 

from collections import OrderedDict 

from director import fieldcontainer 

from director import transformUtils 

 

import json 

 

class ConstraintEncoder(nje.NumpyConvertEncoder): 

    def default(self, obj): 

        if isinstance(obj, vtk.vtkTransform): 

            pos, quat = transformUtils.poseFromTransform(obj) 

            return OrderedDict(position=pos, quaternion=quat) 

        elif isinstance(obj, fieldcontainer.FieldContainer): 

            d = OrderedDict() 

            d['class'] = type(obj).__name__ 

            for key in obj._fields: 

                d[key] = getattr(obj, key) 

            return d 

        return nje.NumpyConvertEncoder.default(self, obj) 

 

 

def ConstraintDecoder(dct): 

    return nje.NumpyDecoder(dct) 

 

def encodeConstraints(dataObj, **kwargs): 

    return json.dumps(dataObj, cls=ConstraintEncoder, **kwargs) 

 

def decodeConstraints(dataStream): 

    return json.loads(dataStream, object_hook=ConstraintDecoder) 

 

def getPlanPoses(constraints, ikPlanner): 

    ''' 

    Given a list of constraints, returns a dictionary of poses containing all 

    the poses that are references by the constraints by name 

    ''' 

 

    poses = sorted([c.postureName for c in constraints if hasattr(c, 'postureName')]) 

    poses = {poseName:list(ikPlanner.jointController.getPose(poseName)) for poseName in poses} 

    return poses