Coverage for /home/travis/build/RobotLocomotion/director/build/src/director-build/lib/python2.7/dist-packages/director/irisUtils : 0%

import numpy as np 

import drc as lcmdrc 

from irispy.utils import lcon_to_vert 

from director.lcmframe import frameFromPositionMessage, positionMessageFromFrame 

from scipy.spatial import ConvexHull 

class SafeTerrainRegion: 

    __slots__ = ['A', 'b', 'C', 'd','pose'] 

    """ 

    Container for describing an IRIS safe region of terrain. 

    Format: 

        A, b: linear constraints on [x,y,z,yaw] of the center of the foot, A[x;y;z;yaw] <= b 

        C, d: ellipsoidal constraints on [x,y,z,yaw] of the center of the foot. Not currently used. 

        tform: a pose on the terrain within the region, from which we extract a plane. 

    """ 

    def __init__(self, A, b, C, d, tform): 

        self.A = A 

        self.b = b 

        self.C = C 

        self.d = d 

        self.tform = tform 

    def to_iris_region_t(self): 

        msg = lcmdrc.iris_region_t() 

        msg.lin_con = self.to_lin_con_t() 

        msg.seed_pose = positionMessageFromFrame(self.tform) 

        return msg 

    def to_lin_con_t(self): 

        msg = encodeLinCon(self.A, self.b) 

        return msg 

    def xy_polytope(self): 

        """ 

        Project the 3D c-space polytope Ax <= b to a list of vertices in the xy plane 

        """ 

        V = lcon_to_vert(self.A, self.b) 

        if V is not None and V.size > 0: 

            hull = ConvexHull(V[:2,:].T) 

            return V[:2,hull.vertices] 

        else: 

            # print "Infeasible polytope" 

            return np.zeros((2,0)) 

    @staticmethod 

    def from_iris_region_t(msg): 

        A, b = decodeLinCon(msg.lin_con) 

        C = [] 

        d = [] 

        tform = frameFromPositionMessage(msg.seed_pose) 

        return SafeTerrainRegion(A, b, C, d, tform) 

    @property 

    def point(self): 

        point = self.tform.GetPosition() 

        print "point", point 

        return self.tform.GetPosition() 

    @property 

    def normal(self): 

        return self.tform.TransformNormal(np.array([0,0,1])) 

def encodeLinCon(A, b): 

    A = np.asarray(A) 

    b = np.asarray(b) 

    msg = lcmdrc.lin_con_t() 

    msg.m = A.shape[0] 

    msg.n = A.shape[1] 

    msg.m_times_n = msg.m * msg.n 

    msg.A = A.flatten(order='F') # column-major 

    msg.b = b 

    return msg 

def decodeLinCon(msg): 

    A = np.asarray(msg.A) 

    b = np.asarray(msg.b) 

    A = A.reshape((msg.m, msg.n), order='F') 

    return A, b