from pydrake.geometry import SceneGraph
from pydrake.math import RollPitchYaw
from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder, VectorSystem
from pydrake.systems.meshcat_visualizer import MeshcatVisualizer
from pydrake.systems.rendering import MultibodyPositionToGeometryPose
from pydrake.systems.primitives import AffineSystem
from pydrake.attic.multibody.rigid_body_tree import RigidBodyTree, AddModelInstanceFromUrdfFile, FloatingBaseType
from pydrake.attic.multibody.rigid_body_plant import RigidBodyPlant
LOWER_BOUND = [-5, -5, -3]
UPPER_BOUND = [5, 5, 3]
RADIUS = .25 # maybe .175
OBJECTS = [([1, 1, 1], np.array([0, 0, 0]), rm([1, 0, 0], 0))]
bounds = irispy.Polyhedron.fromBounds(LOWER_BOUND, UPPER_BOUND)
obstacles = []
for object_ in OBJECTS:
sz, trans, _ = object_
sz_x, sz_y, sz_z = sz
trans_x, trans_y, trans_z = trans
obstacles.append(
np.array([
[
trans_x + (sz_x + RADIUS), trans_y + (sz_y + RADIUS),
trans_z + (sz_z + RADIUS)
],
[
trans_x + (sz_x + RADIUS), trans_y - (sz_y + RADIUS),
trans_z + (sz_z + RADIUS)
from controller import QuadrotorController, PPTrajectory
from controller import rotation_matrix as rm
from pydrake.geometry import SceneGraph
from pydrake.math import RollPitchYaw
from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder, VectorSystem
from pydrake.systems.meshcat_visualizer import MeshcatVisualizer
from pydrake.systems.rendering import MultibodyPositionToGeometryPose
from pydrake.systems.primitives import AffineSystem
from pydrake.attic.multibody.rigid_body_tree import RigidBodyTree, AddModelInstanceFromUrdfFile, FloatingBaseType
from pydrake.attic.multibody.rigid_body_plant import RigidBodyPlant
OBJECTS = [
([.2, .2, 1], np.array([2, -2, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([2, 0, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([2, 1, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([0, -2, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([0, 0, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([0, -1, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([0, 2, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([-1, -1, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([-2, 1, 0]), rm([1, 0, 0], 0)),
]
START_STATE = np.zeros(12) #np.random.randn(12,)
START_STATE[0] = 4.0
GOAL_STATE = np.zeros(12)
GOAL_STATE[0] = -4.0
DURATION = 4.0
from controller import QuadrotorController, PPTrajectory
from controller import rotation_matrix as rm
from pydrake.geometry import SceneGraph
from pydrake.math import RollPitchYaw
from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder, VectorSystem
from pydrake.systems.meshcat_visualizer import MeshcatVisualizer
from pydrake.systems.rendering import MultibodyPositionToGeometryPose
from pydrake.systems.primitives import AffineSystem
from pydrake.attic.multibody.rigid_body_tree import RigidBodyTree, AddModelInstanceFromUrdfFile, FloatingBaseType
from pydrake.attic.multibody.rigid_body_plant import RigidBodyPlant
OBJECTS = [
([.2, 2, 2], np.array([.75, 0, .5]), rm([1, 0, 0], 0)),
([.2, 2, 2], np.array([-.75, 0, -.5]), rm([1, 0, 0], 0)),
]
START_STATE = np.zeros(12) #np.random.randn(12,)
START_STATE[0] = 4.0
GOAL_STATE = np.zeros(12)
GOAL_STATE[0] = -4.0
DURATION = 4.0
sample_times = np.linspace(0, DURATION, 8)
num_vars = 3
degree = 5
continuity_degree = 4
tf = DURATION
y_traj = PPTrajectory(sample_times, num_vars, degree, continuity_degree)