<commit_before>from __future__ import division

import numpy as np
from scipy.spatial import ConvexHull

from ddapp.irisUtils import SafeTerrainRegion
from ddapp import transformUtils
from irispy.utils import sample_convex_polytope
import polyhedron._cdd
from polyhedron import Vrep, Hrep
from py_drake_utils.utils import rpy2rotmat




DEFAULT_FOOT_CONTACTS = np.array([[-0.13, -0.13, 0.13, 0.13],
                                  [0.0562, -0.0562, 0.0562, -0.0562]])
DEFAULT_BOUNDING_BOX_WIDTH = 1


class PolygonSegmentationNonIRIS():
    def __init__(self, polygon_vertices, bot_pts=DEFAULT_FOOT_CONTACTS,
                 bounding_box_width=DEFAULT_BOUNDING_BOX_WIDTH):

        polygon_vertices = np.asarray(polygon_vertices)
        self.planar_polyhedron = Vrep(polygon_vertices[:2,:].T)
        self.bot_pts = bot_pts

    def getBoundingPolytope(self, start):
        """
        Return A, b describing a bounding box on [x, y, yaw] into which the IRIS region must be contained.
        The format is A [x;y;yaw] <= b
        """
        start = np.array(start).reshape((3,))
        lb = np.hstack((start[:2] - self.bounding_box_width / 2, start[2] - np.pi))
        ub = np.hstack((start[:2] + self.bounding_box_width / 2, start[2] + np.pi))
        A_bounds = np.vstack((-np.eye(3), np.eye(3)))
        b_bounds = np.hstack((-lb, ub))
        return A_bounds, b_bounds

    def findSafeRegion(self, pose):
        pose = np.asarray(pose)
        tform = transformUtils.frameFromPositionAndRPY([0,0,0], pose[3:] * 180 / np.pi)

        contact_pts_on_plane = np.zeros((2, self.bot_pts.shape[1]))
        for j in range(self.bot_pts.shape[1]):
            contact_pts_on_plane[:,j] = tform.TransformPoint([self.bot_pts[0,j], self.bot_pts[1,j], 0])[:2]

        Rdot = np.array([[0, -1], [1, 0]])
        contact_vel_in_world = Rdot.dot(contact_pts_on_plane)

        c_region = {'A': [], 'b': []}

        for i in range(self.planar_polyhedron.A.shape[0]):
            ai = self.planar_polyhedron.A[i,:]<commit_msg>fix transform definition in terrain findSafeRegion<commit_after>from __future__ import division

import numpy as np
from scipy.spatial import ConvexHull

from ddapp.irisUtils import SafeTerrainRegion
from ddapp import transformUtils
from irispy.utils import sample_convex_polytope
import polyhedron._cdd
from polyhedron import Vrep, Hrep
from py_drake_utils.utils import rpy2rotmat




DEFAULT_FOOT_CONTACTS = np.array([[-0.13, -0.13, 0.13, 0.13],
                                  [0.0562, -0.0562, 0.0562, -0.0562]])
DEFAULT_BOUNDING_BOX_WIDTH = 1


class PolygonSegmentationNonIRIS():
    def __init__(self, polygon_vertices, bot_pts=DEFAULT_FOOT_CONTACTS,
                 bounding_box_width=DEFAULT_BOUNDING_BOX_WIDTH):

        polygon_vertices = np.asarray(polygon_vertices)
        self.planar_polyhedron = Vrep(polygon_vertices[:2,:].T)
        self.bot_pts = bot_pts

    def getBoundingPolytope(self, start):
        """
        Return A, b describing a bounding box on [x, y, yaw] into which the IRIS region must be contained.
        The format is A [x;y;yaw] <= b
        """
        start = np.array(start).reshape((3,))
        lb = np.hstack((start[:2] - self.bounding_box_width / 2, start[2] - np.pi))
        ub = np.hstack((start[:2] + self.bounding_box_width / 2, start[2] + np.pi))
        A_bounds = np.vstack((-np.eye(3), np.eye(3)))
        b_bounds = np.hstack((-lb, ub))
        return A_bounds, b_bounds

    def findSafeRegion(self, pose):
        pose = np.asarray(pose)
        tformForProjection = transformUtils.frameFromPositionAndRPY([0,0,0], pose[3:] * 180 / np.pi)
        tform = transformUtils.frameFromPositionAndRPY(pose[:3], pose[3:] * 180 / np.pi)

        contact_pts_on_plane = np.zeros((2, self.bot_pts.shape[1]))
        for j in range(self.bot_pts.shape[1]):
            contact_pts_on_plane[:,j] = tformForProjection.TransformPoint([self.bot_pts[0,j], self.bot_pts[1,j], 0])[:2]

        Rdot = np.array([[0, -1], [1, 0]])
        contact_vel_in_world = Rdot.dot(contact_pts_on_plane)

        c_region = {'A': [], 'b': []}

        for i in range(self.planar_polyhedron.A.shape[0]):
            ai = self.planar_polyhedron.A[i,:]
{'fraction_non_alphanumeric': 0.12981455064194009, 'fraction_numerical': 0.03114598193057537, 'mean_word_length': 3.0413064361191164, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4982711', 'n_tokens_mistral': 1603, 'n_tokens_neox': 1491, 'n_words': 303}