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, draw_collision=True)
    return rbt, pbrv

 def test_populate_upstream(self):
     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 test_populate_from_folder(self):
     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", ""))

 def test_populate_from_environment(self):
     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"]

    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)
        robot_3 = RigidBodyTree()
        AddModelInstancesFromSdfFile(
            sdf_file,
            floating_base_type,
            weld_frame,
            robot_3)

        for robot in robot_1, robot_2, robot_3:
            expected_num_bodies = 4
            self.assertEqual(robot.get_num_bodies(), expected_num_bodies)

    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))

 def test_constructor(self):
     pm = PackageMap()
     model = os.path.join(getDrakePath(), "examples", "Atlas", "urdf",
                          "atlas_minimal_contact.urdf")
     pm.PopulateUpstreamToDrake(model)
     robot = rbtree.RigidBodyTree(
         model, package_map=pm,
         floating_base_type=rbtree.FloatingBaseType.kRollPitchYaw)

    def testCoM0(self):
        r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))

        kinsol = r.doKinematics(np.zeros((7,1)), np.zeros((7,1)))

        c = r.centerOfMass(kinsol)

        self.assertTrue(np.allclose(c.flat, [0.0, 0.0, -0.2425], atol=1e-4))

    def test_value(self):
        r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))
        self.assertEqual(r.number_of_positions(), 7)
        self.assertEqual(r.number_of_velocities(), 7)

        kinsol = r.doKinematics(np.zeros((7,1)), np.zeros((7,1)))

        p = r.transformPoints(kinsol, np.zeros((3,1)), 0, 1)
        self.assertTrue(np.allclose(p, np.zeros((3,1))))

    def test_relative_transform(self):
        r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(),
                                         "examples/Pendulum/Pendulum.urdf"))

        q = np.zeros(7)
        q[6] = np.pi / 2
        kinsol = r.doKinematics(q)
        T = r.relativeTransform(kinsol, 1, 2)
        self.assertTrue(np.allclose(T,
                                    np.array([[0, 0, 1, 0],
                                              [0, 1, 0, 0],
                                              [-1, 0, 0, 0],
                                              [0, 0, 0, 1]])))

    def resolvePackageFilename(filename):

        if not packagepath.PackageMap.isPackageUrl(filename):
            return filename

        if Geometry.PackageMap is None:
            import pydrake
            m = packagepath.PackageMap()
            m.populateFromSearchPaths([pydrake.getDrakePath()])
            m.populateFromEnvironment(['DRAKE_PACKAGE_PATH', 'ROS_PACKAGE_PATH'])
            Geometry.PackageMap = m

        return Geometry.PackageMap.resolveFilename(filename) or filename

    def test_big_gradient(self):
        r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))

        q = toAutoDiff(np.zeros((7,1)), np.eye(7, 100))
        v = toAutoDiff(np.zeros((7,1)), np.zeros((7, 100)))
        kinsol = r.doKinematics(q, v)

        point = np.ones((3,1))
        p = r.transformPoints(kinsol, point, 2, 0)
        self.assertTrue(np.allclose(p.value(), np.ones((3,1))))
        self.assertTrue(np.allclose(p.derivatives()[:3,:7],
                                    np.array([[1, 0, 0, 0, 1, -1, 1],
                                              [0, 1, 0, -1, 0, 1, 0],
                                              [0, 0, 1, 1, -1, 0, -1]])))

    def testPostureConstraint(self):
        r = pydrake.rbtree.RigidBodyTree(
            os.path.join(pydrake.getDrakePath(),
                         "examples/Pendulum/Pendulum.urdf"))
        q = -0.9
        posture_constraint = ik.PostureConstraint(r)
        posture_constraint.setJointLimits(np.array([[6]], dtype=np.int32),
                                          np.array([[q]]),
                                          np.array([[q]]))
        # Choose a seed configuration (randomly) and a nominal configuration
        # (at 0)
        q_seed = np.vstack((np.zeros((6, 1)),
                            0.8147))
        q_nom = np.vstack((np.zeros((6, 1)),
                           0.))

        options = ik.IKoptions(r)
        results = ik.InverseKin(
            r, q_seed, q_nom, [posture_constraint], options)
        self.assertEqual(results.info[0], 1)
        self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9)

        # Run the tests again both pointwise and as a trajectory to
        # validate the interfaces.
        t = np.array([0., 1.])
        q_seed_array = np.transpose(np.array([q_seed, q_seed]))[0]
        q_nom_array = np.transpose(np.array([q_nom, q_nom]))[0]

        results = ik.InverseKinPointwise(r, t, q_seed_array, q_nom_array,
                                         [posture_constraint], options)
        self.assertEqual(len(results.info), 2)
        self.assertEqual(len(results.q_sol), 2)
        self.assertEqual(results.info[0], 1)

        # The pointwise result will go directly to the constrained
        # value.
        self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9)
        self.assertEqual(results.info[1], 1)
        self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9)

        results = ik.InverseKinTraj(r, t, q_seed_array, q_nom_array,
                                    [posture_constraint], options)
        self.assertEqual(len(results.info), 1)
        self.assertEqual(len(results.q_sol), 2)
        self.assertEqual(results.info[0], 1)

        # The trajectory result starts at the initial value and moves
        # to the constrained value.
        self.assertAlmostEqual(results.q_sol[0][6], q_seed[6], 1e-9)
        self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9)

    def testCoMJacobian(self):
        r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(),
                                         "examples/Pendulum/Pendulum.urdf"))
        q = r.getRandomConfiguration()
        kinsol = r.doKinematics(q, np.zeros((7, 1)))
        J = r.centerOfMassJacobian(kinsol)

        self.assertTrue(np.shape(J) == (3, 7))

        q = r.getZeroConfiguration()
        kinsol = r.doKinematics(q, np.zeros((7, 1)))
        J = r.centerOfMassJacobian(kinsol)

        self.assertTrue(np.allclose(J.flat, [1., 0., 0., 0., -0.2425, 0., -0.25,
        0., 1., 0., 0.2425, 0., 0., 0.,
        0., 0., 1., 0., 0., 0., 0.], atol=1e-4))

    def testPostureConstraint(self):
        r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))
        q = -0.9
        posture_constraint = ik.PostureConstraint(r)
        posture_constraint.setJointLimits(np.array([[6]], dtype=np.int32),
                                          np.array([[q]]),
                                          np.array([[q]]))
        # Choose a seed configuration (randomly) and a nominal configuration (at 0)
        q_seed = np.vstack((np.zeros((6,1)),
                            0.8147))
        q_nom = np.vstack((np.zeros((6,1)),
                       0.))

        options = ik.IKoptions(r)
        results = ik.inverseKinSimple(r,
                                      q_seed,
                                      q_nom,
                                      [posture_constraint],
                                      options)
        self.assertAlmostEqual(results.q_sol[6], q, 1e-9)

    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)

    def test_gradient(self):
        r = pydrake.rbtree.RigidBodyTree(
            os.path.join(pydrake.getDrakePath(),
                         "examples/Pendulum/Pendulum.urdf"),
            pydrake.rbtree.FloatingBaseType.kRollPitchYaw)

        def do_transform(q):
            kinsol = r.doKinematics(q)
            point = np.ones((3, 1))
            return r.transformPoints(kinsol, point, 2, 0)

        q = np.zeros(7)

        value = do_transform(q)
        self.assertTrue(np.allclose(value, np.ones((3, 1))))

        g = fd.jacobian(do_transform, q)
        self.assertTrue(np.allclose(g,
                                    np.array([[[1, 0, 0, 0, 1, -1, 1]],
                                              [[0, 1, 0, -1, 0, 1, 0]],
                                              [[0, 0, 1, 1, -1, 0, -1]]])))

    def testAddModelInstanceFromUrdfStringSearchingInRosPackages(self):
        urdf_file = os.path.join(pydrake.getDrakePath(),
                                 "examples/PR2/pr2.urdf")
        urdf_string = open(urdf_file).read()
        base_dir = os.path.dirname(urdf_file)
        package_map = pydrake.rbtree.PackageMap()
        weld_frame = None
        floating_base_type = pydrake.rbtree.kRollPitchYaw

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

        expected_num_bodies = 83
        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))

    def test_api(self):
        box_size = [1., 2., 3.]
        radius = 0.1
        length = 0.2

        box = shapes.Box(size=box_size)
        self.assertTrue(np.allclose(box.size, box_size))
        self.assertEqual(box.getPoints().shape, (3, 8))
        self.assertEqual(len(box.getFaces()), 12)
        self.assertEqual(len(box.getFaces()[0]), 3)

        sphere = shapes.Sphere(radius=radius)
        self.assertEqual(sphere.radius, radius)
        self.assertEqual(sphere.getPoints().shape, (3, 1))
        with self.assertRaises(RuntimeError):
            sphere.getFaces()

        cylinder = shapes.Cylinder(radius=radius, length=length)
        self.assertEqual(cylinder.radius, radius)
        self.assertEqual(cylinder.length, length)

        capsule = shapes.Capsule(radius=radius, length=length)
        self.assertEqual(capsule.radius, radius)
        self.assertEqual(capsule.length, length)

        pts = np.tile(box_size, (10, 1)).T
        mesh_points = shapes.MeshPoints(pts)
        self.assertEqual(mesh_points.getPoints().shape, (3, 10))

        obj_mesh_path = os.path.join(
            pydrake.getDrakePath(), "examples/quadrotor/quadrotor_base.obj")
        obj_mesh_uri = "box_obj"
        mesh = shapes.Mesh(uri=obj_mesh_uri, resolved_filename=obj_mesh_path)
        self.assertTrue(np.allclose(mesh.scale, [1., 1., 1.]))
        self.assertEqual(mesh.uri, obj_mesh_uri)
        self.assertEqual(mesh.resolved_filename, obj_mesh_path)

    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 directory 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"))