Example #1
0
    def __init__(self,
                 builder,
                 dt=5e-4,
                 N=150,
                 params=None,
                 trj_decay=0.7,
                 x_w_cov=1e-5,
                 door_angle_ref=1.0,
                 visualize=False):
        self.plant_derivs = MultibodyPlant(time_step=dt)
        parser = Parser(self.plant_derivs)
        self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs,
                                                 parser,
                                                 params=params)
        self.plant_derivs.Finalize()
        self.plant_derivs_context = self.plant_derivs.CreateDefaultContext()
        self.plant_derivs.get_actuation_input_port().FixValue(
            self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.])
        null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
        self.plant_derivs.GetInputPort("applied_generalized_force").FixValue(
            self.plant_derivs_context, null_force)

        self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
                                                              time_step=dt)
        parser = Parser(self.plant, scene_graph)
        self.iiwa, self.hinge, self.bushing = self.add_models(self.plant,
                                                              parser,
                                                              params=params)
        self.plant.Finalize(
        )  # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor.

        self.meshcat = ConnectMeshcatVisualizer(builder,
                                                scene_graph,
                                                zmq_url=zmq_url)

        self.sym_derivs = False  # If system should use symbolic derivatives; if false, autodiff
        self.custom_sim = False  # If rollouts should be gathered with sys.dyn() calls

        nq = self.plant.num_positions()
        nv = self.plant.num_velocities()
        self.n_x = nq + nv
        self.n_u = self.plant.num_actuators()
        self.n_y = self.plant.get_state_output_port(self.iiwa).size()

        self.N = N
        self.dt = dt
        self.decay = trj_decay
        self.V = 1e-2 * np.ones(self.n_y)
        self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv)))
        self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv)))
        self.x_w_cov = x_w_cov
        self.door_angle_ref = door_angle_ref

        self.q0 = np.array(
            [-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
        #self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
        self.x0 = np.concatenate((self.q0, np.zeros(nv)))
        self.door_index = None

        self.phi = {}
Example #2
0
    def test_thigh_torque_return_type(self):
        """Verify the signature of ChooseThighTorque"""
        from hopper_2d import Hopper2dController

        builder = DiagramBuilder()
        plant = builder.AddSystem(MultibodyPlant(0.0005))
        parser = Parser(plant)
        parser.AddModelFromFile("raibert_hopper_2d.sdf")
        plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
        plant.Finalize()
        controller = Hopper2dController(plant, desired_lateral_velocity=0.0)

        x0 = np.zeros(10)
        x0[1] = 4.  # in air
        x0[4] = 0.5  # feasible leg length
        x0[5] = 0.1  # initial speed

        torquedes = controller.ChooseThighTorque(x0)
        self.assertIsInstance(torquedes, float,
            "ChooseThighTorque returned a type other than "\
            "float for X0 = %s, desired_lateral_velocity = %f" %
                (np.array_str(x0), controller.desired_lateral_velocity))

        # Try from another desired velocity
        controller.desired_lateral_velocity = -1.0
        torquedes = controller.ChooseThighTorque(x0)
        self.assertIsInstance(torquedes, float,
            "ChooseThighTorque returned a type other than "\
            "float for X0 = %s, desired_lateral_velocity = %f" %
                (np.array_str(x0), controller.desired_lateral_velocity))
def compile_scene_tree_clearance_geometry_to_mbp_and_sg(
        scene_tree, timestep=0.001, alpha=0.25):
    builder = DiagramBuilder()
    mbp, scene_graph = AddMultibodyPlantSceneGraph(
        builder, MultibodyPlant(time_step=timestep))
    parser = Parser(mbp)
    parser.package_map().PopulateFromEnvironment("ROS_PACKAGE_PATH")
    world_body = mbp.world_body()
    free_body_poses = []
    # For generating colors.
    node_class_to_color_dict = {}
    cmap = plt.cm.get_cmap('jet')
    cmap_counter = 0.
    for node in scene_tree.nodes:
        if node.tf is not None and node.physics_geometry_info is not None:
            # Don't have to do anything if this does not introduce geometry.
            sanity_check_node_tf_and_physics_geom_info(node)
            phys_geom_info = node.physics_geometry_info
            has_clearance_geometry = len(phys_geom_info.clearance_geometry) > 0
            if not has_clearance_geometry:
                continue

            # Add a body for this node and register the clearance geometry.
            # TODO(gizatt) This tree body index is built in to disambiguate names.
            # But I forsee a name-to-stuff resolution crisis when inference time comes...
            # this might get resolved by the solution to that.
            body = mbp.AddRigidBody(name=node.name,
                                    M_BBo_B=phys_geom_info.spatial_inertia)
            tf = torch_tf_to_drake_tf(node.tf)
            mbp.SetDefaultFreeBodyPose(body, tf)

            # Pick out a color for this class.
            node_type_string = node.__class__.__name__
            if node_type_string in node_class_to_color_dict.keys():
                color = node_class_to_color_dict[node_type_string]
            else:
                color = list(cmap(cmap_counter))
                color[3] = alpha
                node_class_to_color_dict[node_type_string] = color
                cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.)

            # Handle adding primitive geometry by adding it all to one
            # mbp.
            if len(phys_geom_info.clearance_geometry) > 0:
                for k, (tf, geometry) in enumerate(
                        phys_geom_info.clearance_geometry):
                    mbp.RegisterCollisionGeometry(
                        body=body,
                        X_BG=torch_tf_to_drake_tf(tf),
                        shape=geometry,
                        name=node.name + "_col_%03d" % k,
                        coulomb_friction=default_friction)
                    mbp.RegisterVisualGeometry(body=body,
                                               X_BG=torch_tf_to_drake_tf(tf),
                                               shape=geometry,
                                               name=node.name +
                                               "_vis_%03d" % k,
                                               diffuse_color=color)

    return builder, mbp, scene_graph
def runPendulumExample(args):
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
    parser = Parser(plant)
    parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf"))
    plant.Finalize()

    pose_bundle_output_port = scene_graph.get_pose_bundle_output_port()
    Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
                     dtype=np.float64)
    visualizer = builder.AddSystem(
        PlanarSceneGraphVisualizer(scene_graph,
                                   Tview=Tview,
                                   xlim=[-1.2, 1.2],
                                   ylim=[-1.2, 1.2]))
    builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0))

    diagram = builder.Build()
    simulator = Simulator(diagram)
    simulator.Initialize()
    simulator.set_target_realtime_rate(1.0)

    # Fix the input port to zero.
    plant_context = diagram.GetMutableSubsystemContext(
        plant, simulator.get_mutable_context())
    plant_context.FixInputPort(plant.get_actuation_input_port().get_index(),
                               np.zeros(plant.num_actuators()))
    plant_context.SetContinuousState([0.5, 0.1])
    simulator.StepTo(args.duration)
Example #5
0
def CreateIiwaControllerPlant():
    """creates plant that includes only the robot and gripper, used for controllers."""
    robot_sdf_path = FindResourceOrThrow(
        "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf")
    gripper_sdf_path = FindResourceOrThrow(
        "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf")
    sim_timestep = 1e-3
    plant_robot = MultibodyPlant(sim_timestep)
    parser = Parser(plant=plant_robot)
    parser.AddModelFromFile(robot_sdf_path)
    parser.AddModelFromFile(gripper_sdf_path)
    plant_robot.WeldFrames(
        A=plant_robot.world_frame(),
        B=plant_robot.GetFrameByName("iiwa_link_0"))
    plant_robot.WeldFrames(
        A=plant_robot.GetFrameByName("iiwa_link_7"),
        B=plant_robot.GetFrameByName("body"),
        X_AB=RigidTransform(RollPitchYaw(np.pi/2, 0, np.pi/2), np.array([0, 0, 0.114]))
    )
    plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0])
    plant_robot.Finalize()

    link_frame_indices = []
    for i in range(8):
        link_frame_indices.append(
            plant_robot.GetFrameByName("iiwa_link_" + str(i)).index())

    return plant_robot, link_frame_indices
Example #6
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
def build_block_diagram(actuators_off=False,
                        desired_lateral_velocity=0.0,
                        desired_height=3.0,
                        print_period=0.0):
    builder = DiagramBuilder()

    # Build the plant
    plant = builder.AddSystem(MultibodyPlant(0.0005))
    scene_graph = builder.AddSystem(SceneGraph())
    plant.RegisterAsSourceForSceneGraph(scene_graph)
    builder.Connect(plant.get_geometry_poses_output_port(),
                    scene_graph.get_source_pose_port(plant.get_source_id()))
    builder.Connect(scene_graph.get_query_output_port(),
                    plant.get_geometry_query_input_port())

    # Build the robot
    parser = Parser(plant)
    parser.AddModelFromFile("raibert_hopper_2d.sdf")
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
    plant.Finalize()
    plant.set_name('plant')

    # Create a logger to log at 30hz
    state_dim = plant.num_positions() + plant.num_velocities()
    state_log = builder.AddSystem(SignalLogger(state_dim))
    state_log.DeclarePeriodicPublish(0.0333, 0.0)  # 30hz logging
    builder.Connect(plant.get_state_output_port(), state_log.get_input_port(0))
    state_log.set_name('state_log')

    # The controller
    controller = builder.AddSystem(
        Hopper2dController(plant,
                           desired_lateral_velocity=desired_lateral_velocity,
                           desired_height=desired_height,
                           actuators_off=actuators_off,
                           print_period=print_period))
    builder.Connect(plant.get_state_output_port(),
                    controller.get_input_port(0))
    builder.Connect(controller.get_output_port(0),
                    plant.get_actuation_input_port())
    controller.set_name('controller')

    # Create visualizer
    visualizer = builder.AddSystem(
        PlanarSceneGraphVisualizer(scene_graph,
                                   xlim=[-1, 10],
                                   ylim=[-.2, 4.5],
                                   show=False))
    builder.Connect(scene_graph.get_pose_bundle_output_port(),
                    visualizer.get_input_port(0))
    visualizer.set_name('visualizer')

    diagram = builder.Build()

    return diagram
Example #8
0
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False):
    builder = DiagramBuilder()
    station = builder.AddSystem(ManipulationStation(time_step=1e-3))
    station.SetupClutterClearingStation()
    if manipuland is "foam":
        station.AddManipulandFromFile(
            "drake/examples/manipulation_station/models/061_foam_brick.sdf",
            T_world_objectInitial)
    elif manipuland is "ball":
        station.AddManipulandFromFile(
            "drake/examples/manipulation_station/models/sphere.sdf",
            T_world_objectInitial)
    elif manipuland is "bball":
        station.AddManipulandFromFile(
            "drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery
            T_world_objectInitial)
    elif manipuland is "rod":
        station.AddManipulandFromFile(
            "drake/examples/manipulation_station/models/rod.sdf",
            T_world_objectInitial)
    station_plant = station.get_multibody_plant()
    parser = Parser(station_plant)
    if include_bin:
        parser.AddModelFromFile("extra_bin.sdf")
        station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin)
    if include_hoop:
        parser.AddModelFromFile("sdfs/hoop.sdf")
        station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin)
    station.Finalize()

    frames_to_draw = {"gripper": {"body"}}
    meshcat = None
    if zmq_url is not None:
        meshcat = ConnectMeshcatVisualizer(builder,
            station.get_scene_graph(),
            output_port=station.GetOutputPort("pose_bundle"),
            delete_prefix_on_load=False,
            frames_to_draw=frames_to_draw,
            zmq_url=zmq_url)

    diagram = builder.Build()

    plant = station.get_multibody_plant()
    context = plant.CreateDefaultContext()
    gripper = plant.GetBodyByName("body")

    initial_pose = plant.EvalBodyPoseInWorld(context, gripper)

    simulator = Simulator(diagram)
    simulator.set_target_realtime_rate(1.0)
    simulator.AdvanceTo(0.01)

    return initial_pose, meshcat
    def RunSimulation(self, real_time_rate=1.0):
        '''
        Here we test using the NNSystem in a Simulator to drive
        an acrobot.
        '''
        sdf_file = "assets/acrobot.sdf"
        urdf_file = "assets/acrobot.urdf"

        builder = DiagramBuilder()
        plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
        parser = Parser(plant=plant, scene_graph=scene_graph)
        parser.AddModelFromFile(sdf_file)
        plant.Finalize(scene_graph)

        # Add
        nn_system = NNSystem(self.pytorch_nn_object)
        builder.AddSystem(nn_system)

        # NN -> plant
        builder.Connect(nn_system.NN_out_output_port,
                        plant.get_actuation_input_port())
        # plant -> NN
        builder.Connect(plant.get_continuous_state_output_port(),
                        nn_system.NN_in_input_port)

        # Add meshcat visualizer
        meshcat = MeshcatVisualizer(scene_graph)
        builder.AddSystem(meshcat)
        # builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
        builder.Connect(scene_graph.get_pose_bundle_output_port(),
                        meshcat.GetInputPort("lcm_visualization"))

        # build diagram
        diagram = builder.Build()
        meshcat.load()
        # time.sleep(2.0)
        RenderSystemWithGraphviz(diagram)

        # construct simulator
        simulator = Simulator(diagram)

        # context = diagram.GetMutableSubsystemContext(
        #     self.station, simulator.get_mutable_context())

        simulator.set_publish_every_time_step(False)
        simulator.set_target_realtime_rate(real_time_rate)
        simulator.Initialize()
        sim_duration = 5.
        simulator.StepTo(sim_duration)
        print("stepping complete")
Example #10
0
def Simulate2dHopper(x0,
                     duration,
                     desired_lateral_velocity=0.0,
                     print_period=0.0):
    builder = DiagramBuilder()

    plant = builder.AddSystem(MultibodyPlant(0.0005))
    scene_graph = builder.AddSystem(SceneGraph())
    plant.RegisterAsSourceForSceneGraph(scene_graph)
    builder.Connect(plant.get_geometry_poses_output_port(),
                    scene_graph.get_source_pose_port(plant.get_source_id()))
    builder.Connect(scene_graph.get_query_output_port(),
                    plant.get_geometry_query_input_port())

    # Build the plant
    parser = Parser(plant)
    parser.AddModelFromFile("raibert_hopper_2d.sdf")
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
    plant.AddForceElement(UniformGravityFieldElement())
    plant.Finalize()

    # Create a logger to log at 30hz
    state_dim = plant.num_positions() + plant.num_velocities()
    state_log = builder.AddSystem(SignalLogger(state_dim))
    state_log.DeclarePeriodicPublish(0.0333, 0.0)  # 30hz logging
    builder.Connect(plant.get_continuous_state_output_port(),
                    state_log.get_input_port(0))

    # The controller
    controller = builder.AddSystem(
        Hopper2dController(plant,
                           desired_lateral_velocity=desired_lateral_velocity,
                           print_period=print_period))
    builder.Connect(plant.get_continuous_state_output_port(),
                    controller.get_input_port(0))
    builder.Connect(controller.get_output_port(0),
                    plant.get_actuation_input_port())

    # The diagram
    diagram = builder.Build()
    simulator = Simulator(diagram)
    simulator.Initialize()

    plant_context = diagram.GetMutableSubsystemContext(
        plant, simulator.get_mutable_context())
    plant_context.get_mutable_discrete_state_vector().SetFromVector(x0)

    simulator.StepTo(duration)
    return plant, controller, state_log
def do_generation_and_simulation(sim_time=10):
    vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000")
    scene_tree, satisfied_clearance = rejection_sample_feasible_tree(num_attempts=1000)
    scene_tree, satisfied_feasibility = project_tree_to_feasibility(scene_tree, num_attempts=3)

    serialize_scene_tree_to_yamls_and_sdfs(scene_tree, package_name='save', package_dir=".", remove_directory=True)

    # Draw generated tree in meshcat.
    #draw_scene_tree_meshcat(scene_tree, alpha=1.0, node_sphere_size=0.1)
    # Draw its clearance geometry for debugging.
    #draw_clearance_geometry_meshcat(scene_tree, alpha=0.3)

    # Simulate the resulting scene, with a PR2 for scale.
    builder, mbp, scene_graph = compile_scene_tree_to_mbp_and_sg(
        scene_tree, timestep=0.001)
    # Add PR2 and shift it back in front of where I know the table will be.
    parser = Parser(mbp)
    pr2_model_path = "/home/gizatt/drake/build/install/share/drake/examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf"
    parser.package_map().PopulateUpstreamToDrake(pr2_model_path);
    pr2_model_id = parser.AddModelFromFile(
        file_name=pr2_model_path, model_name="PR2_for_scale")
    # Get the tf of the robot spawn node, and put the PR2 at that xy location.
    robot_spawn_tf = scene_tree.find_nodes_by_type(RobotSpawnLocation)[0].tf.numpy()
    mbp.GetJointByName("x", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[0, 3])
    mbp.GetJointByName("y", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[1, 3])

    mbp.Finalize()
    
    visualizer = ConnectMeshcatVisualizer(builder, scene_graph,
        zmq_url="default")
    diagram = builder.Build()
    diag_context = diagram.CreateDefaultContext()
    mbp_context = diagram.GetMutableSubsystemContext(mbp, diag_context)
    # Fix input port for PR2 to zero.
    actuation_port = mbp.get_actuation_input_port(model_instance=pr2_model_id)
    nu = mbp.num_actuated_dofs(model_instance=pr2_model_id)
    actuation_port.FixValue(mbp_context, np.zeros(nu))


    sim = Simulator(diagram, diag_context)
    sim.set_target_realtime_rate(1.0)
    if not satisfied_clearance:
        print("WARNING: SCENE TREE NOT SATISFYING CLEARANCE")
    if not satisfied_feasibility:
        print("WARNING: SCENE TREE NOT SATISFYING FEASIBILITY, SIM MAY FAIL")
    try:
        sim.AdvanceTo(sim_time)
    except RuntimeError as e:
        print("Encountered error in sim: ", e)
def build_pancake_flipper_plant(builder):
    '''Creates a pancake_flipper MultibodyPlant. Returns the plant and
    corresponding scene graph.

    Inputs:
        builder -- a DiagramBuilder
    Outputs:
        the MultibodyPlant object and corresponding SceneGraph representing
        the pancake flipper
    '''
    # Instantiate the pancake flipper plant and the scene graph.
    # The scene graph is a container for the geometries of all
    # the physical systems in our diagram
    pancake_flipper, scene_graph = AddMultibodyPlantSceneGraph(
        builder,
        time_step=0.0  # discrete update period, or zero for continuous systems
    )

    # parse the urdf and populate the plant
    urdf_path = './models/pancake_flipper_massless_links_nofriction.urdf'
    Parser(pancake_flipper).AddModelFromFile(urdf_path)

    # Finalize the plant so we can use it
    pancake_flipper.Finalize()

    # Return the plant and scene graph, as promised
    return pancake_flipper, scene_graph
def build_iiwa7_plant():
    plant = MultibodyPlant(1e-3)
    parser = Parser(plant=plant)

    iiwa_drake_path = (
        "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf"
    )
    iiwa_path = FindResourceOrThrow(iiwa_drake_path)
    robot_model = parser.AddModelFromFile(iiwa_path)

    # weld robot to world frame.
    plant.WeldFrames(frame_on_parent_P=plant.world_frame(),
                     frame_on_child_C=plant.GetFrameByName("iiwa_link_0"),
                     X_PC=RigidTransform.Identity())
    plant.Finalize()

    return plant
Example #14
0
def AddCameraBox(plant, X_WC, name="camera0", parent_frame=None):
    # TODO(russt): could be smarter and increment the default camera name (by
    # checking with the plant).
    if not parent_frame:
        parent_frame = plant.world_frame()
    camera = Parser(plant).AddModelFromFile(
        FindResource("models/camera_box.sdf"), name)
    plant.WeldFrames(parent_frame, plant.GetFrameByName("base", camera), X_WC)
Example #15
0
def kinematic_tree_example():
    plant = MultibodyPlant(time_step=0.0)
    parser = Parser(plant)

    parser.AddModelFromFile(
        FindResourceOrThrow(
            "drake/manipulation/models/allegro_hand_description/sdf/allegro_hand_description_right.sdf"
        ))

    parser.AddModelFromFile(
        FindResourceOrThrow(
            "drake/examples/manipulation_station/models/061_foam_brick.sdf"))

    plant.Finalize()

    g = Source(plant.GetTopologyGraphvizString())
    print(g.format)
    g.view()
Example #16
0
def AddPlanarIiwa(plant):
    urdf = FindResourceOrThrow(
        "drake/manipulation/models/iiwa_description/urdf/"
        "planar_iiwa14_spheres_dense_elbow_collision.urdf")

    parser = Parser(plant)
    iiwa = parser.AddModelFromFile(urdf)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))

    # Set default positions:
    q0 = [0.1, -1.2, 1.6]
    index = 0
    for joint_index in plant.GetJointIndices(iiwa):
        joint = plant.get_mutable_joint(joint_index)
        if isinstance(joint, RevoluteJoint):
            joint.set_default_angle(q0[index])
            index += 1

    return iiwa
Example #17
0
def AddIiwa(plant, collision_model="no_collision"):
    sdf_path = FindResourceOrThrow(
        "drake/manipulation/models/iiwa_description/iiwa7/"
        f"iiwa7_{collision_model}.sdf")

    parser = Parser(plant)
    iiwa = parser.AddModelFromFile(sdf_path)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))

    # Set default positions:
    q0 = [0.0, 0.1, 0, -1.2, 0, 1.6, 0]
    index = 0
    for joint_index in plant.GetJointIndices(iiwa):
        joint = plant.get_mutable_joint(joint_index)
        if isinstance(joint, RevoluteJoint):
            joint.set_default_angle(q0[index])
            index += 1

    return iiwa
Example #18
0
def setup_dot_diagram(builder, args):
    ''' Using an existing DiagramBuilder, adds a sim of the Dot
    robot. Args comes from argparse.

    The returned controller will need its first port connected to
    a setpoint source.'''

    print(
        "TODO: load in servo calibration dict to a servo calibration object that gets shared"
    )

    with open(args.yaml_path, "r") as f:
        config_dict = yaml.load(f, Loader=Loader)
    sdf_path = os.path.join(os.path.dirname(args.yaml_path),
                            config_dict["sdf_path"])
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0005)
    parser = Parser(plant)
    model = parser.AddModelFromFile(sdf_path)

    # Set initial pose floating above the ground.
    plant.SetDefaultFreeBodyPose(plant.GetBodyByName("body"),
                                 RigidTransform(p=[0., 0., 0.25]))
    if args.welded:
        plant.WeldFrames(plant.world_frame(),
                         plant.GetBodyByName("body").body_frame())
    else:
        add_ground(plant)

    plant.Finalize()

    controller = builder.AddSystem(ServoController(plant, config_dict))
    # Fixed control-rate controller with a low pass filter on its torque output.
    zoh = builder.AddSystem(
        ZeroOrderHold(period_sec=0.001, vector_size=controller.n_servos))
    filter = builder.AddSystem(
        FirstOrderLowPassFilter(time_constant=0.02, size=controller.n_servos))
    builder.Connect(plant.get_state_output_port(),
                    controller.get_input_port(1))
    builder.Connect(controller.get_output_port(0), zoh.get_input_port(0))
    builder.Connect(zoh.get_output_port(0), filter.get_input_port(0))
    builder.Connect(filter.get_output_port(0),
                    plant.get_actuation_input_port())
    return plant, scene_graph, controller
Example #19
0
def grasp_poses_example():
    builder = DiagramBuilder()

    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
    parser = Parser(plant, scene_graph)
    grasp = parser.AddModelFromFile(
        FindResourceOrThrow(
            "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"
        ), "grasp")
    brick = parser.AddModelFromFile(
        FindResourceOrThrow(
            "drake/examples/manipulation_station/models/061_foam_brick.sdf"))
    plant.Finalize()

    meshcat = ConnectMeshcatVisualizer(builder, scene_graph)

    diagram = builder.Build()
    context = diagram.CreateDefaultContext()
    plant_context = plant.GetMyContextFromRoot(context)

    B_O = plant.GetBodyByName("base_link", brick)
    X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)

    B_Ggrasp = plant.GetBodyByName("body", grasp)
    p_GgraspO = [0, 0.12, 0]
    R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply(
        RotationMatrix.MakeZRotation(np.pi / 2.0))

    X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
    X_OGgrasp = X_GgraspO.inverse()
    X_WGgrasp = X_WO.multiply(X_OGgrasp)

    plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp)
    # Open the fingers, too
    plant.GetJointByName("left_finger_sliding_joint",
                         grasp).set_translation(plant_context, -0.054)
    plant.GetJointByName("right_finger_sliding_joint",
                         grasp).set_translation(plant_context, 0.054)

    meshcat.load()
    diagram.Publish(context)
Example #20
0
def AddTwoLinkIiwa(plant, q0=[0.1, -1.2]):
    urdf = FindResource("models/two_link_iiwa14.urdf")

    parser = Parser(plant)
    parser.package_map().Add(
        "iiwa_description",
        os.path.dirname(
            FindResourceOrThrow(
                "drake/manipulation/models/iiwa_description/package.xml")))
    iiwa = parser.AddModelFromFile(urdf)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))

    # Set default positions:
    index = 0
    for joint_index in plant.GetJointIndices(iiwa):
        joint = plant.get_mutable_joint(joint_index)
        if isinstance(joint, RevoluteJoint):
            joint.set_default_angle(q0[index])
            index += 1

    return iiwa
Example #21
0
def num_positions_velocities_example():
    plant = MultibodyPlant(time_step=0.0)
    Parser(plant).AddModelFromFile(
        FindResourceOrThrow(
            "drake/examples/manipulation_station/models/061_foam_brick.sdf"))
    plant.Finalize()

    context = plant.CreateDefaultContext()
    print(context)

    print(f"plant.num_positions() = {plant.num_positions()}")
    print(f"plant.num_velocities() = {plant.num_velocities()}")
Example #22
0
def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("sdf_path", help="path to sdf")
    parser.add_argument("--interactive", action='store_true')
    MeshcatVisualizer.add_argparse_argument(parser)
    args = parser.parse_args()

    builder = DiagramBuilder()
    scene_graph = builder.AddSystem(SceneGraph())
    plant = MultibodyPlant(time_step=0.01)
    plant.RegisterAsSourceForSceneGraph(scene_graph)
    parser = Parser(plant)
    model = parser.AddModelFromFile(args.sdf_path)
    plant.Finalize()

    if args.meshcat:
        meshcat = ConnectMeshcatVisualizer(
            builder, output_port=scene_graph.get_query_output_port(),
            zmq_url=args.meshcat, open_browser=args.open_browser)

    if args.interactive:
        # Add sliders to set positions of the joints.
        sliders = builder.AddSystem(JointSliders(robot=plant))
    else:
        q0 = plant.GetPositions(plant.CreateDefaultContext())
        sliders = builder.AddSystem(ConstantVectorSource(q0))
    to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
    builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
    builder.Connect(
        to_pose.get_output_port(),
        scene_graph.get_source_pose_port(plant.get_source_id()))

    diagram = builder.Build()
    simulator = Simulator(diagram)
    simulator.set_target_realtime_rate(1.0)
    simulator.AdvanceTo(1E6)
Example #23
0
def main():
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
    path = subprocess.check_output([
        'venv/share/drake/common/resource_tool',
        '-print_resource_path',
        'drake/examples/multibody/cart_pole/cart_pole.sdf',
    ]).strip()
    Parser(plant).AddModelFromFile(path)
    plant.Finalize()

    # Add to visualizer.
    DrakeVisualizer.AddToBuilder(builder, scene_graph)

    # Add controller.
    controller = builder.AddSystem(BalancingLQR(plant))
    builder.Connect(plant.get_state_output_port(),
                    controller.get_input_port(0))
    builder.Connect(controller.get_output_port(0),
                    plant.get_actuation_input_port())

    diagram = builder.Build()

    # Set up a simulator to run this diagram.
    simulator = Simulator(diagram)
    simulator.set_target_realtime_rate(1.0)
    context = simulator.get_mutable_context()

    # Simulate.
    duration = 8.0
    for i in range(5):
        initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4)
        print(f"Iteration: {i}. Initial: {initial_state}")
        context.SetContinuousState(initial_state)
        context.SetTime(0.0)
        simulator.Initialize()
        simulator.AdvanceTo(duration)
Example #24
0
def AddWsg(plant, iiwa_model_instance, roll=np.pi / 2.0, welded=False):
    parser = Parser(plant)
    if welded:
        parser.package_map().Add(
            "wsg_50_description",
            os.path.dirname(
                FindResourceOrThrow(
                    "drake/manipulation/models/wsg_50_description/package.xml"))
        )
        gripper = parser.AddModelFromFile(
            FindResource("models/schunk_wsg_50_welded_fingers.sdf"), "gripper")
    else:
        gripper = parser.AddModelFromFile(
            FindResourceOrThrow(
                "drake/manipulation/models/"
                "wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf"))

    X_7G = RigidTransform(RollPitchYaw(np.pi / 2.0, 0, roll), [0, 0, 0.114])
    plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa_model_instance),
                     plant.GetFrameByName("body", gripper), X_7G)
    return gripper
Example #25
0
Then the problem will be reduced to the control of the pendulum only.
"""

# think of the builder as the construction site of our block diagram
builder = DiagramBuilder()

# instantiate the vibrating pendulum and the scene graph
# the scene graph is a container for the geometries of all the physical systems in our diagram
vibrating_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
    builder,
    time_step=0. # discrete update period , set to zero since system is continuous
)

# parse the urdf and populate the vibrating pendulum
urdf_path = FindResource('models/vibrating_pendulum.urdf')
Parser(vibrating_pendulum).AddModelFromFile(urdf_path)
vibrating_pendulum.Finalize()

"""## Write the Controller
In this section we define two controllers:
1. An inner controller that makes the base oscillate with the harmonic motion. We wrote this for you.
2. The outer controller to make the pendulum spin at constant velocity. You will write part of this.

The final diagram will have the following structure:

![figure](https://raw.githubusercontent.com/RussTedrake/underactuated/master/figures/exercises/vibrating_pendulum.jpg)
"""

# this controller enforces the harmonic motion to the base
class InnerController(VectorSystem):
Example #26
0
# unstable equilibrium point
x_star = [0, np.pi, 0, 0]

# weight matrices for the lqr controller
Q = np.eye(4)
R = np.eye(1)

"""Then we construct the block diagram with the cart-pole in closed loop with the LQR controller."""

# start construction site of our block diagram
builder = DiagramBuilder()

# instantiate the cart-pole and the scene graph
cartpole, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = FindResource('models/undamped_cartpole.urdf')
Parser(cartpole).AddModelFromFile(urdf_path)
cartpole.Finalize()

# set the operating point (vertical unstable equilibrium)
context = cartpole.CreateDefaultContext()
context.get_mutable_continuous_state_vector().SetFromVector(x_star)

# fix the input port to zero and get its index for the lqr function
cartpole.get_actuation_input_port().FixValue(context, [0])
input_i = cartpole.get_actuation_input_port().get_index()

# synthesize lqr controller directly from
# the nonlinear system and the operating point
lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i)
lqr = builder.AddSystem(lqr)
Example #27
0
                       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)
        ycb_object_sdfs = [os.path.join(ycb_object_dir, path)
                           for path in ycb_object_sdfs]

        # Add random objects to the scene.
        parser = Parser(mbp, scene_graph)
        n_objects = np.random.randint(1, 12)
        obj_ids = []
        for k in range(n_objects):
            obj_i = np.random.randint(len(ycb_object_sdfs))
            parser.AddModelFromFile(
                file_name=ycb_object_sdfs[obj_i],
                model_name="obj_ycb_%03d" % k)
            obj_ids.append(k+2)

        mbp.Finalize()

        # Optional: set up a meshcat visualizer for the scene.
        #meshcat = builder.AddSystem(
        #    MeshcatVisualizer(scene_graph, draw_period=0.0333))
        #builder.Connect(scene_graph.get_pose_bundle_output_port(),
import argparse
import numpy as np

from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,
                         PlanarSceneGraphVisualizer, Simulator)
from underactuated import FindResource, SliderSystem

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
file_name = FindResource("cartpole/cartpole.urdf")
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()

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

visualizer = builder.AddSystem(
    PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1, 2.5]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))

ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Force", -30., 30.))
builder.Connect(torque_system.get_output_port(0),
                plant.get_actuation_input_port())
Example #29
0
from pydrake.all import (AddMultibodyPlantSceneGraph,
                         DiagramBuilder,
                         Parser,
                         Simulator)
from underactuated import FindResource, PlanarSceneGraphVisualizer

# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)

# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()

builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
                                                          xlim=[-2.8, 2.8],
                                                          ylim=[-2.8, 2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))
diagram = builder.Build()

# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)

# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
Example #30
0
def drake_trajectory_generation(file_name):
    global x_cmd_drake
    global u_cmd_drake
    print(file_name)
    Parser(plant).AddModelFromFile(file_name)
    plant.Finalize()
    context = plant.CreateDefaultContext()
    global dircol 
    dircol= DirectCollocation(
        plant,
        context,
        num_time_samples=11,
        minimum_timestep=0.1,
        maximum_timestep=0.4,
        input_port_index=plant.get_actuation_input_port().get_index())
    dircol.AddEqualTimeIntervalsConstraints()
    initial_state = (0., 0., 0., 0.)
    dircol.AddBoundingBoxConstraint(initial_state, initial_state,
                                dircol.initial_state())
    final_state = (0., math.pi, 0., 0.)
    dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state())
    R = 10  # Cost on input "effort".weight
    u = dircol.input()
    dircol.AddRunningCost(R * u[0]**2)
    # Add a final cost equal to the total duration.
    dircol.AddFinalCost(dircol.time())
    initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
        [0., 4.], np.column_stack((initial_state, final_state)))  # yapf: disable
    dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
    dircol.AddConstraintToAllKnotPoints(dircol.input()[1] <= 0)
    dircol.AddConstraintToAllKnotPoints(dircol.input()[1] >= 0)
    global result
    global u_values
    result = Solve(dircol)
    assert result.is_success()
    #plotphase_portrait()
    fig1, ax1 = plt.subplots()
    u_trajectory = dircol.ReconstructInputTrajectory(result)
    u_knots = np.hstack([
         u_trajectory.value(t) for t in np.linspace(u_trajectory.start_time(),
                                                    u_trajectory.end_time(), 400)
    ])#here the u_knots now is 2x400 
    #u_trajectory = dircol.ReconstructInputTrajectory(result)
    times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400)
    #u_lookup = np.vectorize(u_trajectory.value)
	#now we have ndarray of u_values with 400 points for 4 seconds w/ 100hz pub frequency
    #u_values = u_lookup(times)

    #ax1.plot(times, u_values)
    ax1.plot(times, u_knots[0])
    ax1.plot(times, u_knots[1])
    ax1.set_xlabel("time (seconds)")
    ax1.set_ylabel("force (Newtons)")
    ax1.set_title(' Direct collocation for Cartpole ')
    print('here2')
    plt.show()
    print('here3')
    #x_knots = np.hstack([
    #    x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
                                                 #  x_trajectory.end_time(), 100)
    #])
    x_trajectory = dircol.ReconstructStateTrajectory(result)
    x_knots = np.hstack([
        x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
                                                   x_trajectory.end_time(), 400)
    ])
    print(x_trajectory.start_time())
    print(x_trajectory.end_time())
   

    fig, ax = plt.subplots(4,1,figsize=(8,8))
    plt.subplots_adjust(wspace =0, hspace =0.4)
    #plt.tight_layout(3)#adjust total space
    ax[0].set_title('state of direct collocation for Cartpole')
    ax[0].plot(x_knots[0, :], x_knots[2, :], linewidth=2, color='b', linestyle='-')
    ax[0].set_xlabel("state_dot(theta1_dot and t|heta2_dot)")
    ax[0].set_ylabel("state(theta1 and theta2)");
    ax[0].plot(x_knots[1, :], x_knots[3, :],color='r',linewidth=2,linestyle='--')
    ax[0].legend(('theta1&theta1dot','theta2&theta2dot'));
    ax[1].set_title('input u(t) of direct collocation for Cartpole')
#    ax[1].plot(times,u_values, 'g')
    ax[1].plot(times, u_knots[0])
    ax[1].plot(times, u_knots[1])
    ax[1].legend(('input u(t)'))
    ax[1].set_xlabel("time")
    ax[1].set_ylabel("u(t)")
    ax[1].legend(('x joint ','thetajoint'))
    ax[1].set_title('input x(t) of direct collocation for Cartpole')
    ax[2].plot(times, x_knots[0, :])
    ax[2].set_xlabel("time")
    ax[2].set_ylabel("x(t)")
    ax[2].set_title('input theta(t) of direct collocation for Cartpole')
    ax[3].set_title('input theta(t) of direct collocation for Cartpole')
    ax[3].plot(times, x_knots[1, :])
    ax[3].set_xlabel("time")
    ax[3].set_ylabel("theta(t)")
    print('here4')
    plt.show()
    print('here5')
    x_cmd_drake=x_knots
    #return x_knots[0, :]#u_values
   # u_cmd_drake=u_values
    u_cmd_drake=u_knots