Example #1
0
def setupValkyrieExample():
    # Valkyrie Example
    rbt = RigidBodyTree()
    world_frame = RigidBodyFrame("world_frame", rbt.world(), [0, 0, 0],
                                 [0, 0, 0])
    from pydrake.multibody.parsers import PackageMap
    pmap = PackageMap()
    # Note: Val model is currently not installed in drake binary distribution.
    pmap.PopulateFromFolder(os.path.join(pydrake.getDrakePath(), "examples"))
    # TODO(russt): remove plane.urdf and call AddFlatTerrainTOWorld instead
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
        open(FindResource(os.path.join("underactuated", "plane.urdf")),
             'r').read(),  # noqa
        pmap,
        pydrake.getDrakePath() + "/examples/",
        FloatingBaseType.kFixed,
        world_frame,
        rbt)
    val_start_frame = RigidBodyFrame("val_start_frame", rbt.world(),
                                     [0, 0, 1.5], [0, 0, 0])
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
        open(
            pydrake.getDrakePath() +
            "/examples/valkyrie/urdf/urdf/valkyrie_A_sim_drake_one_neck_dof_wide_ankle_rom.urdf",
            'r').read(),  # noqa
        pmap,
        pydrake.getDrakePath() + "/examples/",
        FloatingBaseType.kRollPitchYaw,
        val_start_frame,
        rbt)
    Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
                     dtype=np.float64)
    pbrv = MeshcatRigidBodyVisualizer(rbt)
    return rbt, pbrv
Example #2
0
    def __init__(self, file_name , Is_fixed = True ):
        rb_tree = RigidBodyTree()
        world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0])
        AddFlatTerrainToWorld(rb_tree, 1000, 10)
        robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0.2], [0, 0, 0])

        if Is_fixed is True:
            self.fixed_type = FloatingBaseType.kFixed
        else:
            self.fixed_type = FloatingBaseType.kRollPitchYaw
        # insert a robot from urdf files
        pmap = PackageMap()
        pmap.PopulateFromFolder(os.path.dirname(file_name))
        AddModelInstanceFromUrdfStringSearchingInRosPackages(
            open(file_name, 'r').read(),
            pmap,
            os.path.dirname(file_name),
            self.fixed_type,  # FloatingBaseType.kRollPitchYaw,
            robot_frame,
            rb_tree)

        self.rb_tree = rb_tree
        self.joint_limit_max = self.rb_tree.joint_limit_max
        self.joint_limit_min = self.rb_tree.joint_limit_min

        print(self.joint_limit_max)
        print(self.joint_limit_min)
Example #3
0
 def test_atlas_parsing(self):
     # Sanity check on parsing.
     pm = PackageMap()
     model = os.path.join(pydrake.getDrakePath(), "examples", "atlas",
                          "urdf", "atlas_minimal_contact.urdf")
     pm.PopulateUpstreamToDrake(model)
     tree = RigidBodyTree(model,
                          package_map=pm,
                          floating_base_type=FloatingBaseType.kRollPitchYaw)
     self.assertEqual(tree.get_num_actuators(), 30)
Example #4
0
    def test_urdf(self):
        """Test that an instance of a URDF model can be loaded into a
        RigidBodyTree by passing a complete set of arguments to Drake's URDF
        parser.
        """
        urdf_file = os.path.join(
            getDrakePath(),
            "examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf")
        with open(urdf_file) as f:
            urdf_string = f.read()
        base_dir = os.path.dirname(urdf_file)
        package_map = PackageMap()
        weld_frame = None
        floating_base_type = FloatingBaseType.kRollPitchYaw

        robot = RigidBodyTree()
        AddModelInstanceFromUrdfStringSearchingInRosPackages(
            urdf_string, package_map, base_dir, floating_base_type, weld_frame,
            robot)

        expected_num_bodies = 86
        self.assertEqual(robot.get_num_bodies(),
                         expected_num_bodies,
                         msg='Incorrect number of bodies: {0} vs. {1}'.format(
                             robot.get_num_bodies(), expected_num_bodies))
Example #5
0
def load_robot_from_urdf(urdf_file):
    """
    This function demonstrates how to pass a complete
    set of arguments to Drake's URDF parser.  It is also
    possible to load a robot with a much simpler syntax
    that uses default values, such as:

      robot = RigidBodyTree(urdf_file)

    """
    urdf_string = open(urdf_file).read()
    base_dir = os.path.dirname(urdf_file)
    package_map = PackageMap()
    weld_frame = None
    floating_base_type = FloatingBaseType.kRollPitchYaw

    # Load our model from URDF
    robot = RigidBodyTree()

    AddModelInstanceFromUrdfStringSearchingInRosPackages(
        urdf_string,
        package_map,
        base_dir,
        floating_base_type,
        weld_frame,
        robot)

    return robot
Example #6
0
 def test_atlas_parsing(self):
     # Sanity check on parsing.
     pm = PackageMap()
     model = FindResourceOrThrow(
         "drake/examples/atlas/urdf/atlas_minimal_contact.urdf")
     pm.PopulateUpstreamToDrake(model)
     tree = RigidBodyTree(model,
                          package_map=pm,
                          floating_base_type=FloatingBaseType.kRollPitchYaw)
     self.assertEqual(tree.get_num_actuators(), 30)
     # Sanity checks joint limits
     #  - Check sizes.
     self.assertEqual(tree.joint_limit_min.size, tree.number_of_positions())
     self.assertEqual(tree.joint_limit_max.size, tree.number_of_positions())
     #  - Check extremal values against values taken from URDF-file. Ignore
     #    the floating-base joints, as they are not present in the URDF.
     self.assertAlmostEqual(np.min(tree.joint_limit_min[6:]), -3.011)
     self.assertAlmostEqual(np.max(tree.joint_limit_max[6:]), 3.14159)
Example #7
0
def SetupLittleDog():
    # Build up your Robot World
    rb_tree = RigidBodyTree()
    world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0])
    AddFlatTerrainToWorld(rb_tree, 1000, 10)
    robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0.2], [0, 0, 0])

    # insert a robot from urdf files
    pmap = PackageMap()
    pmap.PopulateFromFolder(os.path.dirname(model_path))
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
            open(model_path, 'r').read(),
            pmap,
            os.path.dirname(model_path),
            FloatingBaseType.kFixed,  #FloatingBaseType.kRollPitchYaw,
            robot_frame,
            rb_tree)
    return rb_tree
Example #8
0
def SetupKinova():
    # Build up your Robot World
    rb_tree = RigidBodyTree()
    world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0],
                                 [0, 0, 0])
    AddFlatTerrainToWorld(rb_tree, 1000, 10)
    robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
                                 [0, 0, 0])

    # insert a robot from urdf files
    pmap = PackageMap()
    pmap.PopulateFromFolder(os.path.dirname(model_path))
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
        open(model_path, 'r').read(),
        pmap,
        os.path.dirname(model_path),
        FloatingBaseType.kFixed,  #FloatingBaseType.kRollPitchYaw,
        robot_frame,
        rb_tree)

    print("Num of Joints : {}, \n Num of Actuators: {}".format(
        rb_tree.get_num_positions(), rb_tree.get_num_actuators()))
    return rb_tree
Example #9
0
    def test_sdf(self):
        sdf_file = os.path.join(getDrakePath(), "examples/acrobot/Acrobot.sdf")
        with open(sdf_file) as f:
            sdf_string = f.read()
        package_map = PackageMap()
        weld_frame = None
        floating_base_type = FloatingBaseType.kRollPitchYaw

        robot_1 = RigidBodyTree()
        AddModelInstancesFromSdfStringSearchingInRosPackages(
            sdf_string, package_map, floating_base_type, weld_frame, robot_1)
        robot_2 = RigidBodyTree()
        AddModelInstancesFromSdfString(sdf_string, floating_base_type,
                                       weld_frame, robot_2)

        for robot in robot_1, robot_2:
            expected_num_bodies = 4
            self.assertEqual(robot.get_num_bodies(), expected_num_bodies)
Example #10
0
    def test_urdf(self):
        """Test that an instance of a URDF model can be loaded into a
        RigidBodyTree by passing a complete set of arguments to Drake's URDF
        parser.
        """
        urdf_file = os.path.join(
            getDrakePath(),
            "examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf")
        with open(urdf_file) as f:
            urdf_string = f.read()
        base_dir = os.path.dirname(urdf_file)
        package_map = PackageMap()
        weld_frame = None
        floating_base_type = FloatingBaseType.kRollPitchYaw

        robot = RigidBodyTree()
        AddModelInstanceFromUrdfStringSearchingInRosPackages(
            urdf_string, package_map, base_dir, floating_base_type, weld_frame,
            robot)

        expected_num_bodies = 86
        self.assertEqual(robot.get_num_bodies(),
                         expected_num_bodies,
                         msg='Incorrect number of bodies: {0} vs. {1}'.format(
                             robot.get_num_bodies(), expected_num_bodies))

        # Check actuators.
        actuator = robot.GetActuator("head_pan_motor")
        self.assertIsInstance(actuator, RigidBodyActuator)
        self.assertEqual(actuator.name, "head_pan_motor")
        self.assertIs(actuator.body, robot.FindBody("head_pan_link"))
        self.assertEqual(actuator.reduction, 6.0)
        self.assertEqual(actuator.effort_limit_min, -2.645)
        self.assertEqual(actuator.effort_limit_max, 2.645)
        # Check full number of actuators.
        self.assertEqual(len(robot.actuators), robot.get_num_actuators())
        for actuator in robot.actuators:
            self.assertIsInstance(actuator, RigidBodyActuator)
Example #11
0
    def test_package_map(self):
        pm = PackageMap()
        self.assertFalse(pm.Contains("foo"))
        self.assertEqual(pm.size(), 0)
        pm.Add("foo", os.path.abspath(os.curdir))
        self.assertEqual(pm.size(), 1)
        self.assertTrue(pm.Contains("foo"))
        self.assertEqual(pm.GetPath("foo"), os.path.abspath(os.curdir))

        # Populate from folder.
        # TODO(eric.cousineau): This mismatch between casing is confusing, with
        # `Atlas` being the package name, but `atlas` being the dirctory name.
        pm = PackageMap()
        self.assertEqual(pm.size(), 0)
        pm.PopulateFromFolder(
            os.path.join(getDrakePath(), "examples", "atlas"))
        self.assertTrue(pm.Contains("Atlas"))
        self.assertEqual(pm.GetPath("Atlas"), os.path.join(
            getDrakePath(), "examples", "atlas", ""))

        # Populate from environment.
        pm = PackageMap()
        os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"] = os.path.join(
            getDrakePath(), "examples")
        pm.PopulateFromEnvironment("PYDRAKE_TEST_ROS_PACKAGE_PATH")
        self.assertTrue(pm.Contains("Atlas"))
        self.assertEqual(pm.GetPath("Atlas"), os.path.join(
            getDrakePath(), "examples", "atlas", ""))
        del os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"]

        # Populate upstream.
        pm = PackageMap()
        pm.PopulateUpstreamToDrake(
            os.path.join(getDrakePath(), "examples", "atlas", "urdf",
                         "atlas_minimal_contact.urdf"))
        self.assertTrue(pm.Contains("Atlas"))
        self.assertEqual(pm.GetPath("Atlas"), os.path.join(
            getDrakePath(), "examples", "atlas"))
Example #12
0
    def test_package_map(self):
        pm = PackageMap()
        self.assertFalse(pm.Contains("foo"))
        self.assertEqual(pm.size(), 0)
        pm.Add("foo", os.path.abspath(os.curdir))
        self.assertEqual(pm.size(), 1)
        self.assertTrue(pm.Contains("foo"))
        self.assertEqual(pm.GetPath("foo"), os.path.abspath(os.curdir))

        # Populate from folder.
        # TODO(eric.cousineau): This mismatch between casing is confusing, with
        # `Atlas` being the package name, but `atlas` being the dirctory name.
        pm = PackageMap()
        self.assertEqual(pm.size(), 0)
        pm.PopulateFromFolder(os.path.join(getDrakePath(), "examples",
                                           "atlas"))
        self.assertTrue(pm.Contains("Atlas"))
        self.assertEqual(pm.GetPath("Atlas"),
                         os.path.join(getDrakePath(), "examples", "atlas", ""))

        # Populate from environment.
        pm = PackageMap()
        os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"] = os.path.join(
            getDrakePath(), "examples")
        pm.PopulateFromEnvironment("PYDRAKE_TEST_ROS_PACKAGE_PATH")
        self.assertTrue(pm.Contains("Atlas"))
        self.assertEqual(pm.GetPath("Atlas"),
                         os.path.join(getDrakePath(), "examples", "atlas", ""))
        del os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"]

        # Populate upstream.
        pm = PackageMap()
        pm.PopulateUpstreamToDrake(
            os.path.join(getDrakePath(), "examples", "atlas", "urdf",
                         "atlas_minimal_contact.urdf"))
        self.assertTrue(pm.Contains("Atlas"))
        self.assertEqual(pm.GetPath("Atlas"),
                         os.path.join(getDrakePath(), "examples", "atlas"))
def LittleDogSimulationDiagram(lcm, rb_tree, dt, drake_visualizer):
    builder = DiagramBuilder()

    num_pos = rb_tree.get_num_positions()
    num_actuators = rb_tree.get_num_actuators()

    zero_config = np.zeros(
        (rb_tree.get_num_actuators() * 2, 1))  # just for test
    #zero_config =  np.concatenate((np.array([0, 1, 1, 1,   1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
    #zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
    zero_config = np.concatenate(
        (np.array([0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5
                   ]), np.zeros(12)),
        axis=0)
    # zero_config[0] = 1.7
    # zero_config[1] = 1.7
    # zero_config[2] = 1.7
    # zero_config[3] = 1.7
    # RigidBodyPlant
    plant = builder.AddSystem(RigidBodyPlant(rb_tree, dt))
    plant.set_name('plant')

    # Visualizer
    visualizer_publisher = builder.AddSystem(drake_visualizer)
    visualizer_publisher.set_name('visualizer_publisher')
    visualizer_publisher.set_publish_period(0.02)

    builder.Connect(plant.state_output_port(),
                    visualizer_publisher.get_input_port(0))

    # Robot State Encoder
    robot_state_encoder = builder.AddSystem(
        RobotStateEncoder(rb_tree))  # force_sensor_info
    robot_state_encoder.set_name('robot_state_encoder')

    builder.Connect(plant.state_output_port(),
                    robot_state_encoder.joint_state_results_input_port())

    # Robot command to Plant Converter
    robot_command_to_rigidbodyplant_converter = builder.AddSystem(
        RobotCommandToRigidBodyPlantConverter(
            rb_tree.actuators,
            motor_gain=None))  # input argu: rigidbody actuators
    robot_command_to_rigidbodyplant_converter.set_name(
        'robot_command_to_rigidbodyplant_converter')

    builder.Connect(
        robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
        plant.get_input_port(0))

    # PID controller
    kp, ki, kd = joints_PID_params(rb_tree)
    #PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
    #PIDcontroller.set_name('PID_controller')

    rb = RigidBodyTree()
    robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
                                 [0, 0, 0])
    # insert a robot from urdf files
    pmap = PackageMap()
    pmap.PopulateFromFolder(os.path.dirname(model_path))
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
        open(model_path, 'r').read(),
        pmap,
        os.path.dirname(model_path),
        FloatingBaseType.kFixed,  # FloatingBaseType.kRollPitchYaw,
        robot_frame,
        rb)

    PIDcontroller = builder.AddSystem(
        RbtInverseDynamicsController(rb, kp, ki, kd, False))
    PIDcontroller.set_name('PID_controller')

    builder.Connect(robot_state_encoder.joint_state_outport_port(),
                    PIDcontroller.get_input_port(0))

    builder.Connect(
        PIDcontroller.get_output_port(0),
        robot_command_to_rigidbodyplant_converter.robot_command_input_port())

    # Ref trajectory

    traj_src = builder.AddSystem(ConstantVectorSource(zero_config))

    builder.Connect(traj_src.get_output_port(0),
                    PIDcontroller.get_input_port(1))

    # Robot State handle
    robot_state_handle = builder.AddSystem(
        RobotStateHandle(rb_tree))  # force_sensor_info
    robot_state_handle.set_name('robot_state_handle')

    builder.Connect(plant.state_output_port(),
                    robot_state_handle.joint_state_results_input_port())

    logger_N = 4
    logger = []
    for i in range(logger_N):
        logger.append([])

    # Signal logger
    signalLogRate = 100
    logger[0] = builder.AddSystem(SignalLogger(num_pos * 2))
    logger[0]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
    builder.Connect(plant.get_output_port(0), logger[0].get_input_port(0))

    # cotroller command
    logger[1] = builder.AddSystem(SignalLogger(num_actuators))
    logger[1]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
    builder.Connect(PIDcontroller.get_output_port(0),
                    logger[1].get_input_port(0))

    # torque

    # other
    logger[2] = builder.AddSystem(SignalLogger(3))
    builder.Connect(robot_state_handle.com_outport_port(),
                    logger[2].get_input_port(0))
    logger[2]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)

    logger[3] = builder.AddSystem(SignalLogger(num_actuators))
    builder.Connect(plant.torque_output_port(), logger[3].get_input_port(0))
    logger[3]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)

    return builder.Build(), logger, plant
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer):
    builder = DiagramBuilder()

    num_pos = rb_tree.get_num_positions()
    num_actuators = rb_tree.get_num_actuators()

    zero_config = np.zeros(
        (rb_tree.get_num_actuators() * 2, 1))  # just for test
    # zero_config =  np.concatenate((np.array([0, 1, 1, 1,   1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
    # zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
    zero_config = np.concatenate(
        (np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0)
    # zero_config[0] = 1.7
    # zero_config[1] = 1.7
    # zero_config[2] = 1.7
    # zero_config[3] = 1.7

    # 1.SceneGraph system
    scene_graph = builder.AddSystem(SceneGraph())
    scene_graph.RegisterSource('scene_graph_n')

    plant = builder.AddSystem(MultibodyPlant())
    plant_parser = Parser(plant, scene_graph)
    plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog")
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body"))

    # Add gravity to the model.
    plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
    # Model is completed
    plant.Finalize()

    builder.Connect(scene_graph.get_query_output_port(),
                    plant.get_geometry_query_input_port())
    builder.Connect(plant.get_geometry_poses_output_port(),
                    scene_graph.get_source_pose_port(plant.get_source_id()))
    ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)

    # Robot State Encoder
    robot_state_encoder = builder.AddSystem(
        RobotStateEncoder(rb_tree))  # force_sensor_info
    robot_state_encoder.set_name('robot_state_encoder')

    builder.Connect(plant.get_continuous_state_output_port(),
                    robot_state_encoder.joint_state_results_input_port())

    # Robot command to Plant Converter
    robot_command_to_rigidbodyplant_converter = builder.AddSystem(
        RobotCommandToRigidBodyPlantConverter(
            rb_tree.actuators,
            motor_gain=None))  # input argu: rigidbody actuators
    robot_command_to_rigidbodyplant_converter.set_name(
        'robot_command_to_rigidbodyplant_converter')

    builder.Connect(
        robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
        plant.get_actuation_input_port())

    # PID controller
    kp, ki, kd = joints_PID_params(rb_tree)
    # PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
    # PIDcontroller.set_name('PID_controller')

    rb = RigidBodyTree()
    robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
                                 [0, 0, 0])
    # insert a robot from urdf files
    pmap = PackageMap()
    pmap.PopulateFromFolder(os.path.dirname(model_path))
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
        open(model_path, 'r').read(),
        pmap,
        os.path.dirname(model_path),
        FloatingBaseType.kFixed,  # FloatingBaseType.kRollPitchYaw,
        robot_frame,
        rb)

    PIDcontroller = builder.AddSystem(
        RbtInverseDynamicsController(rb, kp, ki, kd, False))
    PIDcontroller.set_name('PID_controller')

    builder.Connect(robot_state_encoder.joint_state_outport_port(),
                    PIDcontroller.get_input_port(0))

    builder.Connect(
        PIDcontroller.get_output_port(0),
        robot_command_to_rigidbodyplant_converter.robot_command_input_port())

    # Ref trajectory

    traj_src = builder.AddSystem(ConstantVectorSource(zero_config))

    builder.Connect(traj_src.get_output_port(0),
                    PIDcontroller.get_input_port(1))

    # Signal logger
    logger = builder.AddSystem(SignalLogger(num_pos * 2))
    builder.Connect(plant.get_continuous_state_output_port(),
                    logger.get_input_port(0))

    return builder.Build(), logger, plant
        rb_tree.actuators, motor_gain=None))  # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
    'robot_command_to_rigidbodyplant_converter')

builder.Connect(
    robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
    plant.get_input_port(0))

# PID controller
kp, ki, kd = joints_PID_params(rb_tree)

rb = rigid_body_tree = RigidBodyTree()
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
                             [0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
    open(model_path, 'r').read(),
    pmap,
    os.path.dirname(model_path),
    FloatingBaseType.kFixed,  #FloatingBaseType.kRollPitchYaw,
    robot_frame,
    rb)

#PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
controller = RbtInverseDynamicsController(rb, kp, ki, kd, False)
PIDcontroller = builder.AddSystem(controller)
PIDcontroller.set_name('PID_controller')

controller_test = builder.AddSystem(Robot_controller_test(rb_tree, kp, ki, kd))