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 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
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_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_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 setup_kuka(self, rbt): iiwa_urdf_path = os.path.join( pydrake.getDrakePath(), "manipulation", "models", "iiwa_description", "urdf", "iiwa14_polytope_collision.urdf") wsg50_sdf_path = os.path.join( pydrake.getDrakePath(), "manipulation", "models", "wsg_50_description", "sdf", "schunk_wsg_50.sdf") table_sdf_path = os.path.join( pydrake.getDrakePath(), "examples", "kuka_iiwa_arm", "models", "table", "extra_heavy_duty_table_surface_only_collision.sdf") guillotine_path = "guillotine.sdf" AddFlatTerrainToWorld(rbt) table_frame_robot = RigidBodyFrame( "table_frame_robot", rbt.world(), [0.0, 0, 0], [0, 0, 0]) self.add_model_wrapper(table_sdf_path, FloatingBaseType.kFixed, table_frame_robot, rbt) self.tabletop_indices.append(rbt.get_num_bodies()-1) table_frame_fwd = RigidBodyFrame( "table_frame_fwd", rbt.world(), [0.7, 0, 0], [0, 0, 0]) self.add_model_wrapper(table_sdf_path, FloatingBaseType.kFixed, table_frame_fwd, rbt) self.tabletop_indices.append(rbt.get_num_bodies()-1) robot_base_frame = RigidBodyFrame( "robot_base_frame", rbt.world(), [0.0, 0, self.table_top_z_in_world], [0, 0, 0]) self.add_model_wrapper(iiwa_urdf_path, FloatingBaseType.kFixed, robot_base_frame, rbt) # Add gripper gripper_frame = rbt.findFrame("iiwa_frame_ee") self.add_model_wrapper(wsg50_sdf_path, FloatingBaseType.kFixed, gripper_frame, rbt) if self.with_knife: # Add guillotine guillotine_frame = RigidBodyFrame( "guillotine_frame", rbt.world(), [0.6, -0.41, self.table_top_z_in_world], [0, 0, 0]) self.add_model_wrapper(guillotine_path, FloatingBaseType.kFixed, guillotine_frame, rbt) self.guillotine_blade_index = \ rbt.FindBody("blade").get_body_index()
def setup_kuka(rbt): iiwa_urdf_path = os.path.join(pydrake.getDrakePath(), "manipulation", "models", "iiwa_description", "urdf", "iiwa14_polytope_collision.urdf") wsg50_sdf_path = os.path.join(pydrake.getDrakePath(), "manipulation", "models", "wsg_50_description", "sdf", "schunk_wsg_50.sdf") table_sdf_path = os.path.join( pydrake.getDrakePath(), "examples", "kuka_iiwa_arm", "models", "table", "extra_heavy_duty_table_surface_only_collision.sdf") object_urdf_path = os.path.join(pydrake.getDrakePath(), "examples", "kuka_iiwa_arm", "models", "objects", "block_for_pick_and_place.urdf") AddFlatTerrainToWorld(rbt) table_frame_robot = RigidBodyFrame("table_frame_robot", rbt.world(), [0.0, 0, 0], [0, 0, 0]) AddModelInstancesFromSdfString( open(table_sdf_path).read(), FloatingBaseType.kFixed, table_frame_robot, rbt) table_frame_fwd = RigidBodyFrame("table_frame_fwd", rbt.world(), [0.8, 0, 0], [0, 0, 0]) AddModelInstancesFromSdfString( open(table_sdf_path).read(), FloatingBaseType.kFixed, table_frame_fwd, rbt) table_top_z_in_world = 0.736 + 0.057 / 2 robot_base_frame = RigidBodyFrame("robot_base_frame", rbt.world(), [0.0, 0, table_top_z_in_world], [0, 0, 0]) AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed, robot_base_frame, rbt) object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(), [0.8, 0, table_top_z_in_world + 0.1], [0, 0, 0]) AddModelInstanceFromUrdfFile(object_urdf_path, FloatingBaseType.kRollPitchYaw, object_init_frame, rbt) # Add gripper gripper_frame = rbt.findFrame("iiwa_frame_ee") AddModelInstancesFromSdfString( open(wsg50_sdf_path).read(), FloatingBaseType.kFixed, gripper_frame, rbt)
def test(): import os import pydrake from director import roboturdf filename = os.path.join(pydrake.getDrakePath(), 'examples/PR2/pr2.urdf') assert os.path.isfile(filename) robotModel, jointController = roboturdf.loadRobotModel(urdfFile=filename, view=view, useConfigFile=False) conf = { 'pr2LeftGripper': [0.07], 'pr2RightArm': [-0.91698, 0.042600, -1.5, -2.01531, -1.57888, -1.65300, -2.04511], 'pr2Base': [0.0, 0.0, 0.0], 'pr2Torso': [0.3], 'pr2RightGripper': [0.07], 'pr2Head': [0.0, 0.0], 'pr2LeftArm': [2.1, 1.29, 0.0, -0.15, 0.0, -0.1, 0.0] } t = BHPNTranslator(jointController) t.setRobotConf(conf)
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_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_flat_terrain(self): tree = RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf")) # Test that AddFlatTerrainToWorld is spelled correctly. AddFlatTerrainToWorld(tree)
def test_kinematics_com_api(self): tree = RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf")) num_q = 7 num_v = 7 q = np.zeros(num_q) v = np.zeros(num_v) self.assertTrue(np.allclose(q, tree.getZeroConfiguration())) kinsol = tree.doKinematics(q, v) # Full center of mass. c = tree.centerOfMass(kinsol) self.assertTrue(np.allclose(c, [0.0, 0.0, -0.2425])) # - Jacobian. Jc = tree.centerOfMassJacobian(kinsol) Jc_expected = np.array([[1., 0., 0., 0., -0.2425, 0., -0.25], [0., 1., 0., 0.2425, 0., 0., 0.], [0., 0., 1., 0., 0., 0., 0.]]) self.assertTrue(np.allclose(Jc, Jc_expected)) # Specific body. arm_com = tree.FindBody("arm_com") c = arm_com.get_center_of_mass() self.assertTrue(np.allclose(c, [0.0, 0.0, -0.5]))
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 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 add_robot(rbt): # type: (RigidBodyTree) -> None # The iiwa link path iiwa_urdf_path = os.path.join(pydrake.getDrakePath(), "manipulation", "models", "iiwa_description", "urdf", "iiwa14_primitive_collision.urdf") robot_base_frame = RigidBodyFrame("robot_base_frame", rbt.world(), [0.0, 0.0, 0.0], [0, 0, 0]) AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed, robot_base_frame, rbt)
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_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)
def addDrakeConfigShortcuts(self, directorConfig): import pydrake drakePath = pydrake.getDrakePath() directorConfig.add_argument( '--iiwa-drake', dest='directorConfigFile', action='store_const', const=os.path.join(drakePath, 'examples/kuka_iiwa_arm/director_config.json'), help='Use KUKA IIWA from drake/examples')
def test_kinematics_api(self): # TODO(eric.cousineau): Reduce these tests to only test API, and do # simple sanity checks on the numbers. tree = RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf")) num_q = 7 num_v = 7 self.assertEqual(tree.number_of_positions(), num_q) self.assertEqual(tree.number_of_velocities(), num_v) q = np.zeros(num_q) v = np.zeros(num_v) # Trivial kinematics. kinsol = tree.doKinematics(q, v) p = tree.transformPoints(kinsol, np.zeros(3), 0, 1) self.assertTrue(np.allclose(p, np.zeros(3))) # AutoDiff jacobians. def do_transform(q): kinsol = tree.doKinematics(q) point = np.ones(3) return tree.transformPoints(kinsol, point, 2, 0) # - Position. value = do_transform(q) self.assertTrue(np.allclose(value, np.ones(3))) # - Gradient. g = jacobian(do_transform, q) g_expected = np.array([[[1, 0, 0, 0, 1, -1, 1]], [[0, 1, 0, -1, 0, 1, 0]], [[0, 0, 1, 1, -1, 0, -1]]]) self.assertTrue(np.allclose(g, g_expected)) # Relative transform. q[:] = 0 q[6] = np.pi / 2 kinsol = tree.doKinematics(q) T = tree.relativeTransform(kinsol, 1, 2) T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) self.assertTrue(np.allclose(T, T_expected)) # Do FK and compare pose of 'arm' with expected pose. q[:] = 0 q[6] = np.pi / 2 kinsol = tree.doKinematics(q) T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm")) T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) self.assertTrue(np.allclose(T, T_expected))
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_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 test_shapes_parsing(self): # TODO(gizatt) This test ought to have its reliance on # the Pendulum model specifics stripped (so that e.g. material changes # don't break the test), and split pure API testing of # VisualElement and Geometry over to shapes_test while keeping # RigidBodyTree and RigidBody visual element extraction here. urdf_path = os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf") tree = RigidBodyTree(urdf_path, floating_base_type=FloatingBaseType.kFixed) # "base_part2" should have a single visual element. base_part2 = tree.FindBody("base_part2") self.assertIsNotNone(base_part2) visual_elements = base_part2.get_visual_elements() self.assertEqual(len(visual_elements), 1) self.assertIsInstance(visual_elements[0], shapes.VisualElement) # It has green material by default sphere_visual_element = visual_elements[0] green_material = np.array([0.3, 0.6, 0.4, 1]) white_material = np.array([1., 1., 1., 1.]) self.assertTrue( np.allclose(sphere_visual_element.getMaterial(), green_material)) sphere_visual_element.setMaterial(white_material) self.assertTrue( np.allclose(sphere_visual_element.getMaterial(), white_material)) # We expect this link TF to have positive z-component... local_tf = sphere_visual_element.getLocalTransform() self.assertAlmostEqual(local_tf[2, 3], 0.015) # ... as well as sphere geometry. self.assertTrue(sphere_visual_element.hasGeometry()) sphere_geometry = sphere_visual_element.getGeometry() self.assertIsInstance(sphere_geometry, shapes.Sphere) self.assertEqual(sphere_geometry.getShape(), shapes.Shape.SPHERE) self.assertNotEqual(sphere_geometry.getShape(), shapes.Shape.BOX) # For a sphere geometry, getPoints() should return # one point at the center of the sphere. sphere_geometry_pts = sphere_geometry.getPoints() self.assertEqual(sphere_geometry_pts.shape, (3, 1)) sphere_geometry_bb = sphere_geometry.getBoundingBoxPoints() self.assertEqual(sphere_geometry_bb.shape, (3, 8)) # Sphere's don't have faces supplied (yet?) self.assertFalse(sphere_geometry.hasFaces()) with self.assertRaises(RuntimeError): sphere_geometry.getFaces()
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 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 test_fk_pose_world_frame(self): # load pendulum robot with fixed base r = pydrake.rbtree.RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf"), pydrake.rbtree.FloatingBaseType.kFixed) # set test configuration (Pendulum robot has single joint 'theta') q = r.getZeroConfiguration() q[0] = np.pi / 2 # do FK and compare pose of 'arm' with expected pose k = r.doKinematics(q) pose_fk = r.CalcBodyPoseInWorldFrame(k, r.FindBody("arm")) pose_true = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]]) self.assertTrue(np.allclose(pose_fk, pose_true))
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_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 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 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 construct_iiwa_simple(): import os import pydrake from pydrake.all import AddFlatTerrainToWorld, RigidBodyFrame, AddModelInstanceFromUrdfFile, FloatingBaseType # The rigid body tree rbt = RigidBodyTree() AddFlatTerrainToWorld(rbt) # The iiwa iiwa_urdf_path = os.path.join( pydrake.getDrakePath(), "manipulation", "models", "iiwa_description", "urdf", "iiwa14_polytope_collision.urdf") robot_base_frame = RigidBodyFrame( "robot_base_frame", rbt.world(), [0.0, 0.0, 0.0], [0, 0, 0]) AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed, robot_base_frame, rbt) return rbt
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_dynamics_api(self): urdf_path = os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf") tree = RigidBodyTree(urdf_path, floating_base_type=FloatingBaseType.kRollPitchYaw) def assert_sane(x, nonzero=True): self.assertTrue(np.all(np.isfinite(x))) if nonzero: self.assertTrue(np.any(x != 0)) num_q = num_v = 7 num_u = tree.get_num_actuators() self.assertEquals(num_u, 1) q = np.zeros(num_q) v = np.zeros(num_v) # Update kinematics. kinsol = tree.doKinematics(q, v) # Sanity checks: # - Actuator map. self.assertEquals(tree.B.shape, (num_v, num_u)) B_expected = np.zeros((num_v, num_u)) B_expected[-1] = 1 self.assertTrue(np.allclose(tree.B, B_expected)) # - Mass matrix. H = tree.massMatrix(kinsol) self.assertEquals(H.shape, (num_v, num_v)) assert_sane(H) self.assertTrue(np.allclose(H[-1, -1], 0.25)) # - Bias terms. C = tree.dynamicsBiasTerm(kinsol, {}) self.assertEquals(C.shape, (num_v, )) assert_sane(C) # - Inverse dynamics. vd = np.zeros(num_v) tau = tree.inverseDynamics(kinsol, {}, vd) assert_sane(tau) # - Friction torques. friction_torques = tree.frictionTorques(v) self.assertTrue(friction_torques.shape, (num_v, )) assert_sane(friction_torques, nonzero=False)
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"))
# Load our model from URDF robot = RigidBodyTree() AddModelInstanceFromUrdfStringSearchingInRosPackages( urdf_string, package_map, base_dir, floating_base_type, weld_frame, robot) return robot urdf_file = os.path.join( pydrake.getDrakePath(), "examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf") # Load our model from URDF robot = load_robot_from_urdf(urdf_file) # Add a convenient frame, positioned 0.1m away from the r_gripper_palm_link # along that link's x axis robot.addFrame(RigidBodyFrame( "r_hand_frame", robot.FindBody("r_gripper_palm_link"), np.array([0.1, 0, 0]), np.array([0., 0, 0]))) # Make sure attribute access works on bodies assert robot.world().get_name() == "world" hand_frame_id = robot.findFrame("r_hand_frame").get_frame_index()
import os import numpy as np import pydrake from pydrake.solvers import ik robot = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf")) # Make sure attribute access works on bodies assert robot.world().name() == "world" constraints = [ik.WorldPositionConstraint(robot, 1, np.zeros((3,)), np.array([0,0,1.0]), np.array([0,0,1.0])), ik.WorldPositionConstraint(robot, robot.findLink("r_gripper_palm_link").body_index, np.zeros((3,)), np.array([0.5,0,1.5]), np.array([0.5,0,1.5])), ] q_seed = robot.getZeroConfiguration() options = ik.IKoptions(robot) results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints, options) print repr(results.q_sol)
def setUp(self): self.r = pydrake.rbtree.RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf"))
floating_base_type = pydrake.rbtree.kRollPitchYaw # Load our model from URDF robot = pydrake.rbtree.RigidBodyTree() pydrake.rbtree.AddModelInstanceFromUrdfStringSearchingInRosPackages( urdf_string, package_map, base_dir, floating_base_type, weld_frame, robot) return robot urdf_file = os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf") # Load our model from URDF robot = load_robot_from_urdf(urdf_file) # Add a convenient frame, positioned 0.1m away from the r_gripper_palm_link # along that link's x axis robot.addFrame( pydrake.rbtree.RigidBodyFrame("r_hand_frame", robot.FindBody("r_gripper_palm_link"), np.array([0.1, 0, 0]), np.array([0., 0, 0]))) # Make sure attribute access works on bodies assert robot.world().get_name() == "world"
import os import numpy as np import pydrake from pydrake.solvers import ik # Load our model from URDF robot = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf")) # Add a convenient frame, positioned 0.1m away from the r_gripper_palm_link # along that link's x axis robot.addFrame( pydrake.rbtree.RigidBodyFrame("r_hand_frame", robot.FindBody("r_gripper_palm_link"), np.array([0.1, 0, 0]), np.array([0., 0, 0]))) # Make sure attribute access works on bodies assert robot.world().get_name() == "world" hand_frame_id = robot.findFrame("r_hand_frame").frame_index base_body_id = robot.FindBody('base_footprint').get_body_index() constraints = [ # These three constraints ensure that the base of the robot is # at z = 0 and has no pitch or roll. Instead of directly # constraining orientation, we just require that the points at # [0, 0, 0], [1, 0, 0], and [0, 1, 0] in the robot's base's # frame must all be at z = 0 in world frame. # We don't care about the x or y position of the robot's base, # so we use NaN values to tell the IK solver not to apply a
def test_api(self): urdf_path = os.path.join( getDrakePath(), "examples/pendulum/Pendulum.urdf") for is_discrete in [False, True]: tree = RigidBodyTree( urdf_path, floating_base_type=FloatingBaseType.kFixed) if is_discrete: timestep = 0.1 plant = mut.RigidBodyPlant(tree, timestep) else: timestep = 0.0 plant = mut.RigidBodyPlant(tree) model_id = 0 # Tested in order in which they're declared in `multibody_py.cc`, # when able. plant.set_contact_model_parameters( mut.CompliantContactModelParameters()) plant.set_default_compliant_material(mut.CompliantMaterial()) self.assertTrue(plant.get_rigid_body_tree() is tree) self.assertEquals(plant.get_num_bodies(), tree.get_num_bodies()) self.assertEquals( plant.get_num_positions(), tree.get_num_positions()) self.assertEquals( plant.get_num_positions(model_id), tree.get_num_positions()) self.assertEquals( plant.get_num_velocities(), tree.get_num_velocities()) self.assertEquals( plant.get_num_velocities(model_id), tree.get_num_velocities()) num_states = plant.get_num_positions() + plant.get_num_velocities() self.assertEquals(plant.get_num_states(), num_states) self.assertEquals(plant.get_num_states(model_id), num_states) num_actuators = 1 self.assertEquals(plant.get_num_actuators(), num_actuators) self.assertEquals(plant.get_num_actuators(model_id), num_actuators) self.assertEquals( plant.get_num_model_instances(), 1) self.assertEquals( plant.get_input_size(), plant.get_num_actuators()) self.assertEquals(plant.get_output_size(), plant.get_num_states()) context = plant.CreateDefaultContext() x = plant.GetStateVector(context) plant.SetDefaultState(context, context.get_mutable_state()) plant.set_position(context, 0, 1.) self.assertEquals(x[0], 1.) plant.set_velocity(context, 0, 2.) self.assertEquals(x[1], 2.) plant.set_state_vector(context, [3., 3.]) self.assertTrue(np.allclose(x, [3., 3.])) plant.set_state_vector(context.get_mutable_state(), [4., 4.]) self.assertTrue(np.allclose(x, [4., 4.])) self.assertEquals( plant.FindInstancePositionIndexFromWorldIndex(0, 0), 0) # Existence checks. self.assertTrue(plant.actuator_command_input_port() is not None) self.assertTrue(plant.model_instance_has_actuators(model_id)) self.assertTrue( plant.model_instance_actuator_command_input_port(model_id) is not None) self.assertTrue( plant.state_output_port() is not None) self.assertTrue( plant.model_instance_state_output_port(model_id) is not None) self.assertTrue( plant.torque_output_port() is not None) self.assertTrue( plant.model_instance_torque_output_port(model_id) is not None) self.assertTrue( plant.kinematics_results_output_port() is not None) self.assertTrue( plant.contact_results_output_port() is not None) # Basic status. self.assertEquals(plant.is_state_discrete(), is_discrete) self.assertEquals(plant.get_time_step(), timestep)