コード例 #1
0
ファイル: drake_dot_sim.py プロジェクト: gizatt/dot
def add_ground(plant):
    ground_model = plant.AddModelInstance("ground_model")
    ground_box = plant.AddRigidBody(
        "ground", ground_model,
        SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1)))
    X_WG = RigidTransform([0, 0, -0.05])
    ground_geometry_id = plant.RegisterCollisionGeometry(
        ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
        CoulombFriction(0.3, 0.25))
    plant.RegisterVisualGeometry(ground_box, RigidTransform(),
                                 Box(10, 10,
                                     0.1), "ground", [0.5, 0.5, 0.5, 1.])
    plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG)
コード例 #2
0
    def __init__(self, name, tf, height, width):
        PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
        # Handle geometry and physics.
        self.wall_thickness = 0.1
        self.width = width
        self.height = height
        # Move the collision geometry so the wall surface is at y=0 (local frame),
        # and is open in the -y direction, and the base of the wall is at z=0.
        geom_tf = pose_to_tf_matrix(
            torch.tensor(
                [0., self.wall_thickness / 2., height / 2., 0., 0., 0.]))
        geometry = Box(width=width, depth=self.wall_thickness, height=height)
        self.register_geometry(geom_tf,
                               geometry,
                               color=np.array([1., 0.898, 0.706, 1.0]))
        # Use the same geom as clearance geometry
        self.register_clearance_geometry(geom_tf, geometry)

        # This node produces a geometric number of cabinets on its surface.
        cabinet_production_rule = RandomRelativePoseProductionRule(
            Cabinet, "%s_cabinet" % name, self._sample_cabinet_pose_on_wall)
        GeometricSetNode.__init__(self,
                                  name=name,
                                  production_rule=cabinet_production_rule,
                                  geometric_prob=0.5)
コード例 #3
0
    def __init__(self, name, tf, width, length):
        PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
        floor_depth = 0.1
        # Move the collision geometry so the surface is at z=0
        geom_tf = pose_to_tf_matrix(
            torch.tensor([0., 0., -floor_depth / 2., 0., 0., 0.]))
        geometry = Box(width=width, depth=length, height=floor_depth)
        self.register_geometry(geom_tf,
                               geometry,
                               color=np.array([0.8, 0.8, 0.8, 1.0]))

        # Spawn a table at a determined location.
        # (Currently just for testing item placement.)
        table_spawn_rule = DeterministicRelativePoseProductionRule(
            child_constructor=Table,
            child_name="%s_table" % name,
            relative_tf=pose_to_tf_matrix(
                torch.tensor([1., 0., 0., 0., 0., 0.])))
        robot_spawn_rule = DeterministicRelativePoseProductionRule(
            child_constructor=RobotSpawnLocation,
            child_name="%s_robot_spawn" % name,
            relative_tf=pose_to_tf_matrix(
                torch.tensor([-1, 0., 0., 0., 0., 0.])))
        AndNode.__init__(self,
                         name=name,
                         production_rules=[table_spawn_rule, robot_spawn_rule])
コード例 #4
0
def AddPlanarBinAndSimpleBox(plant,
                             mass=1.0,
                             mu=1.0,
                             width=0.2,
                             depth=0.05,
                             height=0.3):
    parser = Parser(plant)
    bin = parser.AddModelFromFile(FindResource("models/planar_bin.sdf"))
    plant.WeldFrames(
        plant.world_frame(), plant.GetFrameByName("bin_base", bin),
        RigidTransform(RotationMatrix.MakeZRotation(np.pi / 2.0),
                       [0, 0, -0.015]))

    planar_joint_frame = plant.AddFrame(
        FixedOffsetFrame(
            "planar_joint_frame", plant.world_frame(),
            RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2))))

    # TODO(russt): make this a *random* box?
    # TODO(russt): move random box to a shared py file.
    box_instance = AddShape(plant, Box(width, depth, height), "box", mass, mu)
    box_joint = plant.AddJoint(
        PlanarJoint("box", planar_joint_frame,
                    plant.GetFrameByName("box", box_instance)))
    box_joint.set_position_limits([-.5, -.1, -np.pi], [.5, .3, np.pi])
    box_joint.set_velocity_limits([-2, -2, -2], [2, 2, 2])
    box_joint.set_default_translation([0, depth / 2.0])
    return box_instance
コード例 #5
0
 def __init__(self, name, tf):
     # Handle geometry and physics.
     PhysicsGeometryNodeMixin.__init__(self,
                                       tf=tf,
                                       fixed=True,
                                       is_container=True)
     geom_tf = pose_to_tf_matrix(torch.tensor([0., 0., 0., 0., 0., 0.]))
     # TODO(gizatt) Resource path management to be done here...
     model_path = "/home/gizatt/drake/examples/kuka_iiwa_arm/models/table/extra_heavy_duty_table_surface_only_collision.sdf"
     self.register_model_file(tf=geom_tf,
                              model_path=model_path,
                              root_body_name="link")
     geom_tf = pose_to_tf_matrix(torch.tensor([0., 0., 0.6251, 0., 0., 0.]))
     self.register_clearance_geometry(tf=geom_tf,
                                      geometry=Box(width=1.,
                                                   depth=1.,
                                                   height=1.0))
     # Put an object spawning volume on the table surface.
     rules = []
     rules.append(
         DeterministicRelativePoseProductionRule(
             child_constructor=PlanarObjectRegion,
             child_name="table_object_region",
             relative_tf=pose_to_tf_matrix(
                 torch.tensor([0., 0., 0.8, 0., 0., 0.])),
             object_production_rate=0.5,
             bounds=((-0.25, 0.25), (-0.25, 0.25), (0., 0.2))))
     AndNode.__init__(self, name=name, production_rules=rules)
コード例 #6
0
    def __init__(self, name, tf):
        # Handle geometry and physics.
        PhysicsGeometryNodeMixin.__init__(self,
                                          tf=tf,
                                          fixed=True,
                                          is_container=True)
        # Offset cabinet geometry from the wall
        geom_tf = pose_to_tf_matrix(torch.tensor([0.15, 0., 0., 0., 0., 0.]))
        # TODO(gizatt) Resource path management to be done here...
        model_path = "/home/gizatt/drake/examples/manipulation_station/models/cupboard.sdf"
        # Randomly open doors random amounts.
        # Left door is straight  open at -pi/2 and closed at 0.
        left_door_state = pyro.sample("%s_left_door_state" % name,
                                      dist.Uniform(-np.pi / 2., 0.))
        # Right door is straight open at pi/2 and closed at 0.
        right_door_state = pyro.sample("%s_right_door_state" % name,
                                       dist.Uniform(0.0, np.pi / 2.))
        self.register_model_file(tf=geom_tf,
                                 model_path=model_path,
                                 root_body_name="cupboard_body",
                                 q0_dict={
                                     "left_door_hinge":
                                     left_door_state.detach().numpy(),
                                     "right_door_hinge":
                                     right_door_state.detach().numpy()
                                 })
        # Add clearance geometry to indicate that shelves shouldn't
        # penetrate each other and should have clearance to open the doors.
        clearance_depth = 0.75
        # Offset out from wall just a bit more to give collision detection
        # a margin
        geom_tf = pose_to_tf_matrix(
            torch.tensor(
                [clearance_depth / 2. + 0.001, 0., 0., 0., 0., np.pi / 2.]))
        geometry = Box(width=0.6, depth=clearance_depth, height=1.)
        self.register_clearance_geometry(geom_tf, geometry)

        # Place shelf nodes.
        # Dimensions of a single shelf, in terms of the
        shelf_height = 0.13115 * 2
        bottom_shelf_z_local = -0.3995
        num_shelves = 3
        rules = []
        for k in range(num_shelves):
            rules.append(
                DeterministicRelativePoseProductionRule(
                    child_constructor=PlanarObjectRegion,
                    child_name="cabinet_level_%02d" % k,
                    relative_tf=pose_to_tf_matrix(
                        torch.tensor([
                            0.15, 0., bottom_shelf_z_local + shelf_height * k,
                            0., 0., 0.
                        ])),
                    object_production_rate=0.5,
                    bounds=((-0.1, 0.1), (-0.2, 0.2), (0., 0.2))))
        AndNode.__init__(self, name=name, production_rules=rules)
コード例 #7
0
    def __init__(self, name, tf):
        PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
        # Only has clearance geometry: indicates robot start location, and
        # ensures that there's space to put a robot into this scene.
        geom_tf = pose_to_tf_matrix(torch.tensor([0., 0., 1., 0., 0., 0.]))
        geometry = Box(
            width=1.,
            depth=1.,
            height=2.,
        )
        self.register_clearance_geometry(geom_tf, geometry)

        TerminalNode.__init__(self, name=name)
コード例 #8
0
ファイル: scenarios.py プロジェクト: RussTedrake/manipulation
def AddShape(plant, shape, name, mass=1, mu=1, color=[.5, .5, .9, 1.0]):
    instance = plant.AddModelInstance(name)
    # TODO: Add a method to UnitInertia that accepts a geometry shape (unless
    # that dependency is somehow gross) and does this.
    if isinstance(shape, Box):
        inertia = UnitInertia.SolidBox(shape.width(), shape.depth(),
                                       shape.height())
    elif isinstance(shape, Cylinder):
        inertia = UnitInertia.SolidCylinder(shape.radius(), shape.length())
    elif isinstance(shape, Sphere):
        inertia = UnitInertia.SolidSphere(shape.radius())
    else:
        raise RunTimeError(
            f"need to write the unit inertia for shapes of type {shape}")
    body = plant.AddRigidBody(
        name, instance,
        SpatialInertia(mass=mass,
                       p_PScm_E=np.array([0., 0., 0.]),
                       G_SP_E=inertia))
    if plant.geometry_source_is_registered():
        if isinstance(shape, Box):
            plant.RegisterCollisionGeometry(
                body, RigidTransform(),
                Box(shape.width() - 0.001,
                    shape.depth() - 0.001,
                    shape.height() - 0.001), name, CoulombFriction(mu, mu))
            i = 0
            for x in [-shape.width() / 2.0, shape.width() / 2.0]:
                for y in [-shape.depth() / 2.0, shape.depth() / 2.0]:
                    for z in [-shape.height() / 2.0, shape.height() / 2.0]:
                        plant.RegisterCollisionGeometry(
                            body, RigidTransform([x, y, z]),
                            Sphere(radius=1e-7), f"contact_sphere{i}",
                            CoulombFriction(mu, mu))
                        i += 1
        else:
            plant.RegisterCollisionGeometry(body, RigidTransform(), shape, name,
                                            CoulombFriction(mu, mu))

        plant.RegisterVisualGeometry(body, RigidTransform(), shape, name, color)

    return instance
コード例 #9
0
    def __init__(self,
                 name,
                 tf,
                 object_production_rate,
                 bounds,
                 show_geometry=False):
        PhysicsGeometryNodeMixin.__init__(self, tf=tf, fixed=True)
        self.x_bounds = bounds[0]
        self.y_bounds = bounds[1]
        self.z_bounds = bounds[2]
        # Add some geometry for viz purposes
        geom_tf = pose_to_tf_matrix(
            torch.tensor([
                np.mean(self.x_bounds),
                np.mean(self.y_bounds),
                np.mean(self.z_bounds), 0., 0., 0.
            ]))
        geometry = Box(width=self.x_bounds[1] - self.x_bounds[0],
                       depth=self.y_bounds[1] - self.y_bounds[0],
                       height=self.z_bounds[1] - self.z_bounds[0])
        if show_geometry:
            self.register_visual_geometry(geom_tf,
                                          geometry,
                                          color=np.array([0.5, 1.0, 0.2, 0.2]))

        style_group_options = ["utensils", "foodstuffs"]
        # Do foodstuffs more often than plates and things
        style_group_k = pyro.sample("%s_style" % name,
                                    dist.Categorical(torch.tensor([0.3, 0.7
                                                                   ]))).item()
        style_group = style_group_options[style_group_k]
        # Produce a geometric number of objects within bounds.
        object_production_rule = RandomRelativePoseProductionRule(
            RandomKitchenStuff,
            "%s_object" % name,
            self._sample_object_pose,
            style_group=style_group)
        GeometricSetNode.__init__(self,
                                  name=name,
                                  production_rule=object_production_rule,
                                  geometric_prob=object_production_rate)
コード例 #10
0
ファイル: repro.py プロジェクト: EricCousineau-TRI/repro
def add_geometry(plant, body):
    box = Box(
        width=0.2,
        depth=0.2,
        height=0.2,
    )
    plant.RegisterVisualGeometry(
        body=body,
        X_BG=RigidTransform(),
        shape=box,
        name=f"visual",
        diffuse_color=[1, 1, 1, 1],
    )
    static_friction = 1.
    plant.RegisterCollisionGeometry(body=body,
                                    X_BG=RigidTransform(),
                                    shape=box,
                                    name="collision",
                                    coulomb_friction=CoulombFriction(
                                        static_friction=1,
                                        dynamic_friction=1,
                                    ))
コード例 #11
0
                         SignalLogger, Simulator, VisualElement)
from pydrake.examples.compass_gait import (CompassGait, CompassGaitParams)
from underactuated import (PlanarRigidBodyVisualizer)

tree = RigidBodyTree(
    FindResourceOrThrow("drake/examples/compass_gait/CompassGait.urdf"),
    FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()

builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())

parser = argparse.ArgumentParser()
parser.add_argument("-T",
                    "--duration",
                    type=float,
                    help="Duration to run sim.",
                    default=10.0)
args = parser.parse_args()

visualizer = builder.AddSystem(
    PlanarRigidBodyVisualizer(tree,
コード例 #12
0
if __name__ == "__main__":
    # Random seed setup for reproducibility.
    np.random.seed(int(codecs.encode(os.urandom(4), 'hex'), 32) & (2**32 - 1))
    random.seed(os.urandom(4))

    for scene_k in range(25):

        # Set up a new Drake scene from scratch.
        builder = DiagramBuilder()
        mbp, scene_graph = AddMultibodyPlantSceneGraph(
            builder, MultibodyPlant(time_step=0.005))

        # Add "tabletop" (ground) as a fixed Box welded to the world.
        world_body = mbp.world_body()
        ground_shape = Box(1., 1., 1.)
        ground_body = mbp.AddRigidBody("ground", SpatialInertia(
            mass=10.0, p_PScm_E=np.array([0., 0., 0.]),
            G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
        mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
                       RigidTransform())
        RegisterVisualAndCollisionGeometry(
            mbp, ground_body,
            RigidTransform(p=[0, 0, -0.5]),
            ground_shape, "ground", np.array([0.5, 0.5, 0.5, 1.]),
            CoulombFriction(0.9, 0.8))

        # Figure out what YCB objects we have available to add.
        ycb_object_dir = os.path.join(
            getDrakePath(), "manipulation/models/ycb/sdf/")
        ycb_object_sdfs = os.listdir(ycb_object_dir)