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 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_inverse_dynamics_controller(self): urdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/urdf/iiwa14_primitive_collision.urdf") tree = RigidBodyTree(urdf_path, floating_base_type=FloatingBaseType.kFixed) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = mut.RbtInverseDynamicsController( robot=tree, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = 0 desired_state_port = 1 desired_acceleration_port = 2 control_port = 0 self.assertEqual( controller.get_input_port(desired_acceleration_port).size(), 7) self.assertEqual( controller.get_input_port(estimated_state_port).size(), 14) self.assertEqual( controller.get_input_port(desired_state_port).size(), 14) self.assertEqual(controller.get_output_port(control_port).size(), 7) # current state q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # reference state and acceleration q_r = q + 0.1 * np.ones_like(q) v_r = v + 0.1 * np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp * (q_r - q) + kd * (v_r - v) + ki * integral_term context.FixInputPort(estimated_state_port, BasicVector(x)) context.FixInputPort(desired_state_port, BasicVector(x_r)) context.FixInputPort(desired_acceleration_port, BasicVector(vd_r)) controller.set_integral_value(context, integral_term) # compute the expected torque cache = tree.doKinematics(q, v) expected_torque = tree.massMatrix(cache).dot(vd_d) + \ tree.dynamicsBiasTerm(cache, {}) controller.CalcOutput(context, output) self.assertTrue( np.allclose( output.get_vector_data(0).CopyToVector(), expected_torque))
def test_rgbd_camera(self): sdf_path = FindResourceOrThrow( "drake/systems/sensors/test/models/nothing.sdf") tree = RigidBodyTree() with open(sdf_path) as f: sdf_string = f.read() AddModelInstancesFromSdfString(sdf_string, FloatingBaseType.kFixed, None, tree) frame = RigidBodyFrame("rgbd camera frame", tree.FindBody("link")) tree.addFrame(frame) # Use HDTV size. width = 1280 height = 720 camera = attic_mut.RgbdCamera(name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0, fov_y=np.pi / 4, show_window=False, width=width, height=height) def check_info(camera_info): self.assertIsInstance(camera_info, mut.CameraInfo) self.assertEqual(camera_info.width(), width) self.assertEqual(camera_info.height(), height) check_info(camera.color_camera_info()) check_info(camera.depth_camera_info()) self.assertIsInstance(camera.color_camera_optical_pose(), Isometry3) self.assertIsInstance(camera.depth_camera_optical_pose(), Isometry3) self.assertTrue(camera.tree() is tree) # N.B. `RgbdCamera` copies the input frame. self.assertEqual(camera.frame().get_name(), frame.get_name()) self._check_ports(camera) # Test discrete camera. period = attic_mut.RgbdCameraDiscrete.kDefaultPeriod discrete = attic_mut.RgbdCameraDiscrete(camera=camera, period=period, render_label_image=True) self.assertTrue(discrete.camera() is camera) self.assertTrue(discrete.mutable_camera() is camera) self.assertEqual(discrete.period(), period) self._check_ports(discrete) # That we can access the state as images. context = discrete.CreateDefaultContext() values = context.get_abstract_state() self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U]) self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F]) self.assertIsInstance(values.get_value(2), Value[mut.ImageLabel16I])
def test_accessor_api(self): tree = RigidBodyTree(FindResourceOrThrow( "drake/examples/simple_four_bar/FourBar.urdf")) bodies = tree.get_bodies() self.assertGreater(len(bodies), 0) for body in bodies: self.assertIsInstance(body, RigidBody) frames = tree.get_frames() self.assertGreater(len(frames), 0) for frame in frames: self.assertIsInstance(frame, RigidBodyFrame)
def test_inverse_dynamics(self): urdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/urdf/iiwa14_primitive_collision.urdf") tree = RigidBodyTree(urdf_path, floating_base_type=FloatingBaseType.kFixed) num_v = tree.get_num_velocities() def compute_torque(tree, q, v, v_dot): cache = tree.doKinematics(q, v) return tree.massMatrix(cache).dot(v_dot) + \ tree.dynamicsBiasTerm(cache, {}) estimated_state_port = 0 desired_acceleration_port = 1 def check_torque_example(controller, q, v, v_dot_desired=None): if controller.is_pure_gravity_compensation(): v_dot_desired = np.zeros(num_v) context = controller.CreateDefaultContext() x = np.concatenate([q, v]) context.FixInputPort(estimated_state_port, BasicVector(x)) if not controller.is_pure_gravity_compensation(): context.FixInputPort(desired_acceleration_port, BasicVector(v_dot_desired)) output = controller.AllocateOutput() controller.CalcOutput(context, output) expected_torque = compute_torque(tree, q, v, v_dot_desired) self.assertTrue( np.allclose( output.get_vector_data(0).CopyToVector(), expected_torque)) # Test with pure gravity compensation. controller = mut.RbtInverseDynamics( tree=tree, mode=mut.RbtInverseDynamics.InverseDynamicsMode. kGravityCompensation) # noqa q = np.array([.1, .2, .3, .4, .5, .6, .7]) v = np.zeros(num_v) check_torque_example(controller, q, v) # Test with desired acceleration. controller = mut.RbtInverseDynamics( tree=tree, mode=mut.RbtInverseDynamics.InverseDynamicsMode.kInverseDynamics) q = np.array([.7, .6, .5, .4, .3, .2, .1]) v = np.array([-.1, -.2, -.3, -.4, -.5, -.6, -.7]) v_dot_desired = np.array([-.1, .1, -.1, .1, -.1, .1, -.1]) check_torque_example(controller, q, v, v_dot_desired)
def test_inverse_dynamics(self): urdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/urdf/iiwa14_primitive_collision.urdf") tree = RigidBodyTree(urdf_path, floating_base_type=FloatingBaseType.kFixed) num_v = tree.get_num_velocities() def compute_torque(tree, q, v, v_dot): cache = tree.doKinematics(q, v) return tree.massMatrix(cache).dot(v_dot) + \ tree.dynamicsBiasTerm(cache, {}) estimated_state_port = 0 desired_acceleration_port = 1 def check_torque_example(controller, q, v, v_dot_desired=None): if controller.is_pure_gravity_compensation(): v_dot_desired = np.zeros(num_v) context = controller.CreateDefaultContext() x = np.concatenate([q, v]) context.FixInputPort(estimated_state_port, BasicVector(x)) if not controller.is_pure_gravity_compensation(): context.FixInputPort(desired_acceleration_port, BasicVector(v_dot_desired)) output = controller.AllocateOutput() controller.CalcOutput(context, output) expected_torque = compute_torque(tree, q, v, v_dot_desired) self.assertTrue( np.allclose( output.get_vector_data(0).CopyToVector(), expected_torque)) # Test with pure gravity compensation. controller = mut.RbtInverseDynamics( tree=tree, mode=mut.RbtInverseDynamics.InverseDynamicsMode.kGravityCompensation) # noqa q = np.array([.1, .2, .3, .4, .5, .6, .7]) v = np.zeros(num_v) check_torque_example(controller, q, v) # Test with desired acceleration. controller = mut.RbtInverseDynamics( tree=tree, mode=mut.RbtInverseDynamics.InverseDynamicsMode.kInverseDynamics) q = np.array([.7, .6, .5, .4, .3, .2, .1]) v = np.array([-.1, -.2, -.3, -.4, -.5, -.6, -.7]) v_dot_desired = np.array([-.1, .1, -.1, .1, -.1, .1, -.1]) check_torque_example(controller, q, v, v_dot_desired)
def test_id_table(self): robot = RigidBodyTree() id_table = AddModelInstancesFromSdfFile( FindResourceOrThrow("drake/examples/acrobot/Acrobot.sdf"), FloatingBaseType.kRollPitchYaw, None, robot) # Check IDs. (name, id), = id_table.items() self.assertEqual(name, "Acrobot") self.assertEqual(id, 0) # Ensure that we have our desired base body. base_body_id, = robot.FindBaseBodies(id) expected_body_id = robot.FindBody("base_link").get_body_index() self.assertEqual(base_body_id, expected_body_id)
def test_rgbd_camera(self): sdf_path = FindResourceOrThrow( "drake/systems/sensors/test/models/nothing.sdf") tree = RigidBodyTree() with open(sdf_path) as f: sdf_string = f.read() AddModelInstancesFromSdfString( sdf_string, FloatingBaseType.kFixed, None, tree) frame = RigidBodyFrame("rgbd camera frame", tree.FindBody("link")) tree.addFrame(frame) # Use HDTV size. width = 1280 height = 720 camera = attic_mut.RgbdCamera( name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0, fov_y=np.pi / 4, show_window=False, width=width, height=height) def check_info(camera_info): self.assertIsInstance(camera_info, mut.CameraInfo) self.assertEqual(camera_info.width(), width) self.assertEqual(camera_info.height(), height) check_info(camera.color_camera_info()) check_info(camera.depth_camera_info()) self.assertIsInstance(camera.color_camera_optical_pose(), Isometry3) self.assertIsInstance(camera.depth_camera_optical_pose(), Isometry3) self.assertTrue(camera.tree() is tree) # N.B. `RgbdCamera` copies the input frame. self.assertEqual(camera.frame().get_name(), frame.get_name()) self._check_ports(camera) # Test discrete camera. period = attic_mut.RgbdCameraDiscrete.kDefaultPeriod discrete = attic_mut.RgbdCameraDiscrete( camera=camera, period=period, render_label_image=True) self.assertTrue(discrete.camera() is camera) self.assertTrue(discrete.mutable_camera() is camera) self.assertEqual(discrete.period(), period) self._check_ports(discrete) # That we can access the state as images. context = discrete.CreateDefaultContext() values = context.get_abstract_state() self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U]) self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F]) self.assertIsInstance(values.get_value(2), Value[mut.ImageLabel16I])
def test_kinematics_com_api(self): tree = RigidBodyTree(FindResourceOrThrow( "drake/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)) # - JacobianDotTimesV JcDotV = tree.centerOfMassJacobianDotTimesV(kinsol) self.assertEqual(JcDotV.shape, (3,)) # 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 setupPendulumExample(): rbt = RigidBodyTree(FindResource("pendulum/pendulum.urdf"), floating_base_type=FloatingBaseType.kFixed) # noqa Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) pbrv = PlanarRigidBodyVisualizer(rbt, Tview, [-1.2, 1.2], [-1.2, 1.2]) return rbt, pbrv
def setupDoublePendulumExample(): rbt = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"), floating_base_type=FloatingBaseType.kFixed) # noqa 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 setupDoublePendulumExample(): rbt = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"), floating_base_type=FloatingBaseType.kFixed) # noqa Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) fig, ax = plt.subplots(1, 1) pbrv = PlanarRigidBodyVisualizer(rbt, Tview, [-2.5, 2.5], [-2.5, 2.5], use_random_colors=True, ax=ax) return rbt, pbrv
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)
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 = FindResourceOrThrow( "drake/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() # Add a visual element just to test the spelling of AddVisualElement. tree.world().AddVisualElement(sphere_visual_element) # Test that I can call compile. tree.compile()
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 load_robot(filename, **kwargs): robot = RigidBodyTree() if filename.endswith(".sdf"): AddModelInstancesFromSdfFile( FindResourceOrThrow(filename), FloatingBaseType.kRollPitchYaw, None, robot, **kwargs) else: assert filename.endswith(".urdf") AddModelInstanceFromUrdfFile( FindResourceOrThrow(filename), FloatingBaseType.kRollPitchYaw, None, robot, **kwargs) return robot
def test_frame_api(self): tree = RigidBodyTree(FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf")) # xyz + rpy frame = RigidBodyFrame( name="frame_1", body=tree.world(), xyz=[0, 0, 0], rpy=[0, 0, 0]) self.assertEqual(frame.get_name(), "frame_1") tree.addFrame(frame) self.assertTrue(frame.get_frame_index() < 0, frame.get_frame_index()) self.assertTrue(frame.get_rigid_body() is tree.world()) self.assertIsInstance(frame.get_transform_to_body(), Isometry3) self.assertTrue(tree.findFrame(frame_name="frame_1") is frame)
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 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
def test_dynamics_api(self): urdf_path = FindResourceOrThrow( "drake/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.assertEqual(num_u, 1) q = np.zeros(num_q) v = np.zeros(num_v) # Update kinematics. kinsol = tree.doKinematics(q, v) # AutoDiff q_ad = np.array(list(map(AutoDiffXd, q))) v_ad = np.array(list(map(AutoDiffXd, v))) kinsol_ad = tree.doKinematics(q_ad, v_ad) # Sanity checks: # - Actuator map. self.assertEqual(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) H_ad = tree.massMatrix(kinsol_ad) self.assertEqual(H.shape, (num_v, num_v)) self.assertEqual(H_ad.shape, (num_v, num_v)) assert_sane(H) self.assertTrue(np.allclose(H[-1, -1], 0.25)) # - Bias terms. C = tree.dynamicsBiasTerm(kinsol, {}) C_ad = tree.dynamicsBiasTerm(kinsol_ad, {}) self.assertEqual(C.shape, (num_v,)) self.assertEqual(C_ad.shape, (num_v,)) assert_sane(C) # - Inverse dynamics. vd = np.zeros(num_v) tau = tree.inverseDynamics(kinsol, {}, vd) tau_ad = tree.inverseDynamics(kinsol_ad, {}, vd) self.assertEqual(tau.shape, (num_v,)) self.assertEqual(tau_ad.shape, (num_v,)) assert_sane(tau) # - Friction torques. friction_torques = tree.frictionTorques(v) self.assertEqual(friction_torques.shape, (num_v,)) assert_sane(friction_torques, nonzero=False) # - Centroidal Momentum id = set([0]) Ag = tree.centroidalMomentumMatrix( cache=kinsol, model_instance_id_set=id, in_terms_of_qdot=False) self.assertEqual(Ag.shape, (6, num_q)) AgDotV = tree.centroidalMomentumMatrixDotTimesV( cache=kinsol, model_instance_id_set=id) self.assertEqual(AgDotV.shape, (6,))
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(FindResourceOrThrow( "drake/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))) # Ensure mismatched sizes throw an error. q_bad = np.zeros(num_q + 1) v_bad = np.zeros(num_v + 1) bad_args_list = ( (q_bad,), (q_bad, v), (q, v_bad), ) for bad_args in bad_args_list: with self.assertRaises(SystemExit): tree.doKinematics(*bad_args) # 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)) # Relative RPY, checking autodiff (#9886). q[:] = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7] q_ad = np.array([AutoDiffXd(x) for x in q]) world = tree.findFrame("world") frame = tree.findFrame("arm_com") kinsol = tree.doKinematics(q) rpy = tree.relativeRollPitchYaw( cache=kinsol, from_body_or_frame_ind=world.get_frame_index(), to_body_or_frame_ind=frame.get_frame_index()) kinsol_ad = tree.doKinematics(q_ad) rpy_ad = tree.relativeRollPitchYaw( cache=kinsol_ad, from_body_or_frame_ind=world.get_frame_index(), to_body_or_frame_ind=frame.get_frame_index()) for x, x_ad in zip(rpy, rpy_ad): self.assertEqual(x, x_ad.value()) # 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)) # Geometric Jacobian. # Construct a new tree with a quaternion floating base. tree = RigidBodyTree(FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf"), floating_base_type=FloatingBaseType.kQuaternion) num_q = 8 num_v = 7 self.assertEqual(tree.number_of_positions(), num_q) self.assertEqual(tree.number_of_velocities(), num_v) q = tree.getZeroConfiguration() v = np.zeros(num_v) kinsol = tree.doKinematics(q, v) # - Sanity check sizes. J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0) J_eeDotTimesV = tree.geometricJacobianDotTimesV(kinsol, 0, 2, 0) self.assertEqual(J_default.shape[0], 6) self.assertEqual(J_default.shape[1], num_v) self.assertEqual(len(v_indices_default), num_v) self.assertEqual(J_eeDotTimesV.shape[0], 6) # - Check QDotToVelocity and VelocityToQDot methods q = tree.getZeroConfiguration() v_real = np.zeros(num_v) q_ad = np.array(list(map(AutoDiffXd, q))) v_real_ad = np.array(list(map(AutoDiffXd, v_real))) kinsol = tree.doKinematics(q) kinsol_ad = tree.doKinematics(q_ad) qd = tree.transformVelocityToQDot(kinsol, v_real) v = tree.transformQDotToVelocity(kinsol, qd) qd_ad = tree.transformVelocityToQDot(kinsol_ad, v_real_ad) v_ad = tree.transformQDotToVelocity(kinsol_ad, qd_ad) self.assertEqual(qd.shape, (num_q,)) self.assertEqual(v.shape, (num_v,)) self.assertEqual(qd_ad.shape, (num_q,)) self.assertEqual(v_ad.shape, (num_v,)) v_to_qdot = tree.GetVelocityToQDotMapping(kinsol) qdot_to_v = tree.GetQDotToVelocityMapping(kinsol) v_to_qdot_ad = tree.GetVelocityToQDotMapping(kinsol_ad) qdot_to_v_ad = tree.GetQDotToVelocityMapping(kinsol_ad) self.assertEqual(v_to_qdot.shape, (num_q, num_v)) self.assertEqual(qdot_to_v.shape, (num_v, num_q)) self.assertEqual(v_to_qdot_ad.shape, (num_q, num_v)) self.assertEqual(qdot_to_v_ad.shape, (num_v, num_q)) v_map = tree.transformVelocityMappingToQDotMapping(kinsol, np.eye(num_v)) qd_map = tree.transformQDotMappingToVelocityMapping(kinsol, np.eye(num_q)) v_map_ad = tree.transformVelocityMappingToQDotMapping(kinsol_ad, np.eye(num_v)) qd_map_ad = tree.transformQDotMappingToVelocityMapping(kinsol_ad, np.eye(num_q)) self.assertEqual(v_map.shape, (num_v, num_q)) self.assertEqual(qd_map.shape, (num_q, num_v)) self.assertEqual(v_map_ad.shape, (num_v, num_q)) self.assertEqual(qd_map_ad.shape, (num_q, num_v)) # - Check FindBody and FindBodyIndex methods body_name = "arm" body = tree.FindBody(body_name) self.assertEqual(body.get_body_index(), tree.FindBodyIndex(body_name)) # - Check ChildOfJoint methods body = tree.FindChildBodyOfJoint("theta") self.assertIsInstance(body, RigidBody) self.assertEqual(body.get_name(), "arm") self.assertEqual(tree.FindIndexOfChildBodyOfJoint("theta"), 2) # - Check that default value for in_terms_of_qdot is false. J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \ tree.geometricJacobian(kinsol, 0, 2, 0, False) self.assertTrue((J_default == J_not_in_terms_of_q_dot).all()) self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot) # - Check with in_terms_of_qdot set to True. J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \ tree.geometricJacobian(kinsol, 0, 2, 0, True) self.assertEqual(J_in_terms_of_q_dot.shape[0], 6) self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q) self.assertEqual(len(v_indices_in_terms_of_qdot), num_q) # - Check transformPointsJacobian methods q = tree.getZeroConfiguration() v = np.zeros(num_v) kinsol = tree.doKinematics(q, v) Jv = tree.transformPointsJacobian(cache=kinsol, points=np.zeros(3), from_body_or_frame_ind=0, to_body_or_frame_ind=1, in_terms_of_qdot=False) Jq = tree.transformPointsJacobian(cache=kinsol, points=np.zeros(3), from_body_or_frame_ind=0, to_body_or_frame_ind=1, in_terms_of_qdot=True) JdotV = tree.transformPointsJacobianDotTimesV(cache=kinsol, points=np.zeros(3), from_body_or_frame_ind=0, to_body_or_frame_ind=1) self.assertEqual(Jv.shape, (3, num_v)) self.assertEqual(Jq.shape, (3, num_q)) self.assertEqual(JdotV.shape, (3,)) # - Check that drawKinematicTree runs tree.drawKinematicTree( join(os.environ["TEST_TMPDIR"], "test_graph.dot")) # - Check relative twist method twist = tree.relativeTwist(cache=kinsol, base_or_frame_ind=0, body_or_frame_ind=1, expressed_in_body_or_frame_ind=0) self.assertEqual(twist.shape[0], 6)
def test_inverse_dynamics_controller(self): urdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/urdf/iiwa14_primitive_collision.urdf") tree = RigidBodyTree( urdf_path, floating_base_type=FloatingBaseType.kFixed) kp = np.array([1., 2., 3., 4., 5., 6., 7.]) ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]) kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5]) controller = mut.RbtInverseDynamicsController( robot=tree, kp=kp, ki=ki, kd=kd, has_reference_acceleration=True) context = controller.CreateDefaultContext() output = controller.AllocateOutput() estimated_state_port = 0 desired_state_port = 1 desired_acceleration_port = 2 control_port = 0 self.assertEqual( controller.get_input_port(desired_acceleration_port).size(), 7) self.assertEqual( controller.get_input_port(estimated_state_port).size(), 14) self.assertEqual( controller.get_input_port(desired_state_port).size(), 14) self.assertEqual( controller.get_output_port(control_port).size(), 7) # current state q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3]) v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9]) x = np.concatenate([q, v]) # reference state and acceleration q_r = q + 0.1*np.ones_like(q) v_r = v + 0.1*np.ones_like(v) x_r = np.concatenate([q_r, v_r]) vd_r = np.array([1., 2., 3., 4., 5., 6., 7.]) integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.]) vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term context.FixInputPort(estimated_state_port, BasicVector(x)) context.FixInputPort(desired_state_port, BasicVector(x_r)) context.FixInputPort(desired_acceleration_port, BasicVector(vd_r)) controller.set_integral_value(context, integral_term) # compute the expected torque cache = tree.doKinematics(q, v) expected_torque = tree.massMatrix(cache).dot(vd_d) + \ tree.dynamicsBiasTerm(cache, {}) controller.CalcOutput(context, output) self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(), expected_torque))
def test_flat_terrain(self): tree = RigidBodyTree(FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf")) # Test that AddFlatTerrainToWorld is spelled correctly. AddFlatTerrainToWorld(tree)
def test_rigid_body_tree_programmatic_construction(self): # Tests RBT programmatic construction methods by assembling # a simple RBT with a prismatic and revolute joint, with # both visual and collision geometry on the last joint. rbt = RigidBodyTree() world_body = rbt.world() # body_1 is connected to the world via a prismatic joint along # the +z axis. body_1 = RigidBody() body_1.set_name("body_1") body_1_joint = PrismaticJoint("z", np.eye(4), np.array([0., 0., 1.])) body_1.add_joint(world_body, body_1_joint) rbt.add_rigid_body(body_1) # body_2 is connected to body_1 via a revolute joint around the z-axis. body_2 = RigidBody() body_2.set_name("body_2") body_2_joint = RevoluteJoint("theta", np.eye(4), np.array([0., 0., 1.])) body_2.add_joint(body_1, body_2_joint) box_element = shapes.Box([1.0, 1.0, 1.0]) box_visual_element = shapes.VisualElement( box_element, np.eye(4), [1., 0., 0., 1.]) body_2.AddVisualElement(box_visual_element) body_2_visual_elements = body_2.get_visual_elements() rbt.add_rigid_body(body_2) box_collision_element = CollisionElement(box_element, np.eye(4)) box_collision_element.set_body(body_2) rbt.addCollisionElement(box_collision_element, body_2, "default") # Define a collision filter group containing bodies 1 and 2 and make # that group ignore itself. rbt.DefineCollisionFilterGroup(name="test_group") rbt.AddCollisionFilterGroupMember( group_name="test_group", body_name="body_1", model_id=0) rbt.AddCollisionFilterGroupMember( group_name="test_group", body_name="body_2", model_id=0) rbt.AddCollisionFilterIgnoreTarget("test_group", "test_group") self.assertFalse(rbt.initialized()) rbt.compile() self.assertTrue(rbt.initialized()) # The RBT's position vector should now be [z, theta]. self.assertEqual(body_1.get_position_start_index(), 0) self.assertEqual(body_2.get_position_start_index(), 1) self.assertIsNotNone( rbt.FindCollisionElement( body_2.get_collision_element_ids()[0]))
def test_constraint_api(self): tree = RigidBodyTree(FindResourceOrThrow( "drake/examples/simple_four_bar/FourBar.urdf")) num_q = 9 num_con = 7 bodyA = 1 bodyB = 2 point = np.ones(3) distance = 1 q = tree.getZeroConfiguration() v = np.zeros(num_q) q_ad = np.array(list(map(AutoDiffXd, q))) v_ad = np.array(list(map(AutoDiffXd, v))) kinsol = tree.doKinematics(q, v) kinsol_ad = tree.doKinematics(q_ad, v_ad) self.assertEqual(tree.getNumPositionConstraints(), num_con - 1) tree.addDistanceConstraint(bodyA, point, bodyB, point, distance) self.assertEqual(tree.getNumPositionConstraints(), num_con) constraint = tree.positionConstraints(kinsol) J = tree.positionConstraintsJacobian(kinsol) JdotV = tree.positionConstraintsJacDotTimesV(kinsol) constraint_ad = tree.positionConstraints(kinsol_ad) J_ad = tree.positionConstraintsJacobian(kinsol_ad) JdotV_ad = tree.positionConstraintsJacDotTimesV(kinsol_ad) self.assertEqual(constraint.shape, (num_con,)) self.assertEqual(J.shape, (num_con, num_q)) self.assertEqual(JdotV.shape, (num_con,)) self.assertEqual(constraint_ad.shape, (num_con,)) self.assertEqual(J_ad.shape, (num_con, num_q)) self.assertEqual(JdotV_ad.shape, (num_con,))
def setUp(self): self.r = RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf"))
def _make_tree(self): urdf_path = FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf") tree = RigidBodyTree(urdf_path, floating_base_type=FloatingBaseType.kFixed) return tree
class TestRBTIK(unittest.TestCase): def setUp(self): self.r = RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf")) def testSingleTimeKinematicConstraint(self): # Demonstrate that a subclass of SingleTimeKinematicConstraint # inherits functional `eval` and `bounds` methods. q = np.zeros(self.r.get_num_positions()) kinsol = self.r.doKinematics(q) ub = np.array([0.0, 0.0, -0.45]) lb = np.array([0.0, 0.0, -0.55]) # This point, at the zero configuration, is # at [0, 0, -1] in world frame as well. body_pt = np.array([0., 0., -1]) constraint = ik.WorldPositionConstraint(self.r, self.r.FindBodyIndex("arm"), body_pt, lb, ub) self.assertIsInstance(constraint, ik.SingleTimeKinematicConstraint) lb_inspect, ub_inspect = constraint.bounds(t=0.) self.assertTrue(np.allclose(lb_inspect, lb)) self.assertTrue(np.allclose(ub_inspect, ub)) c, dc_dq = constraint.eval(0., kinsol) # Because the floating base position is all 0's and the arm # origin is not offset from the floating base, the world # point should equal the body point. c_expected = body_pt dc_dq_expected = np.array([[1., 0., 0., 0., -1., 0., -1.], [0., 1., 0., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0., 0.]]) self.assertTrue(np.allclose(c, c_expected)) self.assertTrue(np.allclose(dc_dq, dc_dq_expected)) def testPostureConstraint(self): q = -0.9 posture_constraint = ik.PostureConstraint(self.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(self.r) results = ik.InverseKin(self.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(self.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(self.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 testWorldGazeTargetConstraint(self): model = self.r body = 2 axis = np.ones([3, 1]) target = np.ones([3, 1]) gaze_origin = np.zeros([3, 1]) cone_threshold = 1e-3 tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin, cone_threshold) # Test that construction doesn't fail with a given timespan. ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin, cone_threshold, tspan) def testRelativePositionConstraint(self): model = self.r pts = np.zeros([3, 1]) lb = -np.ones([3, 1]) ub = np.ones([3, 1]) bodyA_idx = 1 bodyB_idx = 2 translation = np.zeros(3) quaternion = np.array([1, 0, 0, 0]) bTbp = np.concatenate([translation, quaternion]).reshape(7, 1) tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx, bTbp) # Test that construction doesn't fail with a given timespan. ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx, bTbp, tspan) def testRelativeGazeDirConstraint(self): model = self.r bodyA_idx = 1 bodyB_idx = 2 axis = np.ones([3, 1]) dir = np.ones([3, 1]) conethreshold = 1e-3 tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir, conethreshold) # Test that construction doesn't fail with a given timespan. ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir, conethreshold, tspan) def testMinDistanceConstraint(self): model = self.r min_distance = 1e-2 active_bodies_idx = list() active_group_name = set() tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.MinDistanceConstraint(model, min_distance, active_bodies_idx, active_group_name) # Test that construction doesn't fail with a given timespan. ik.MinDistanceConstraint(model, min_distance, active_bodies_idx, active_group_name, tspan)
from pydrake.common import FindResourceOrThrow from pydrake.attic.multibody.rigid_body_tree import ( AddModelInstanceFromUrdfFile, FloatingBaseType, RigidBodyTree, RigidBodyFrame, ) from pydrake.systems.framework import BasicVector from pydrake.systems.sensors import CameraInfo from pydrake.attic.systems.sensors import RgbdCamera # Create tree describing scene. urdf_path = FindResourceOrThrow( "drake/multibody/models/box.urdf") tree = RigidBodyTree() AddModelInstanceFromUrdfFile( urdf_path, FloatingBaseType.kFixed, None, tree) # - Add frame for camera fixture. frame = RigidBodyFrame( name="rgbd camera frame", body=tree.world(), xyz=[-2, 0, 2], # Ensure that the box is within range. rpy=[0, np.pi / 4, 0]) tree.addFrame(frame) # Create camera. camera = RgbdCamera( name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0, fov_y=np.pi / 4, show_window=True)
def setUp(self): self.r = RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf"))
def test_rigid_body_tree_collision_api(self): # This test creates a simple RBT with two spheres in simple # collision and verifies collision-detection APIs. rbt = RigidBodyTree() world_body = rbt.world() # Both bodies are unit-diameter spheres # spaced 0.5m from each other along the x axis. for k in range(2): body = RigidBody() body.set_name("body_{}".format(k)) joint = PrismaticJoint("x", np.eye(4), np.array([1.0, 0., 0.])) body.add_joint(world_body, joint) body.set_spatial_inertia(np.eye(6)) rbt.add_rigid_body(body) sphere_element = shapes.Sphere(0.5) sphere_collision_element = CollisionElement( sphere_element, np.eye(4)) sphere_collision_element.set_body(body) rbt.addCollisionElement(sphere_collision_element, body, "default") rbt.compile() self.assertTrue(rbt.initialized()) self.assertEqual(rbt.get_num_positions(), 2) q0 = np.array([-0.25, 0.25]) kinsol = rbt.doKinematics(q0) # One point in each region of overlap: # Sphere one is (), sphere two is [] # p0 ( p2 [ p3 ) p4 ] p5 points = np.array([[-1., -0.6, 0., 0.6, 1.], [0., 0., 0., 0., 0], [0., 0., 0., 0., 0]]) n_pts = points.shape[1] phi, normal, x, body_x, body_idx = rbt.collisionDetectFromPoints( cache=kinsol, points=points, use_margins=False) # Check phi, but leave the other fields as API checks. self.assertTrue( np.allclose(phi, np.array([0.25, -0.15, -0.25, -0.15, 0.25]))) self.assertEqual(phi.shape[0], n_pts) self.assertEqual(normal.shape, (3, n_pts)) self.assertEqual(x.shape, (3, n_pts)) self.assertEqual(normal.shape, (3, n_pts)) point_pairs = rbt.ComputeMaximumDepthCollisionPoints( cache=kinsol, use_margins=False, throw_if_missing_gradient=True) self.assertEqual(len(point_pairs), 1) pp = point_pairs[0] self.assertEqual(rbt.FindCollisionElement(pp.idA), pp.elementA) self.assertEqual(rbt.FindCollisionElement(pp.idB), pp.elementB) possible_points = [np.array([0.5, 0., 0.]), np.array([-0.5, 0.0, 0.0])] for pt in [pp.ptA, pp.ptB]: self.assertTrue( np.allclose(pt, np.array([0.5, 0., 0.])) or np.allclose(pt, np.array([-0.5, 0., 0.]))) self.assertTrue( np.allclose(np.abs(pp.normal), np.array([1., 0., 0.]))) self.assertEqual(pp.distance, -0.5)
class TestRBTIK(unittest.TestCase): def setUp(self): self.r = RigidBodyTree( os.path.join(pydrake.getDrakePath(), "examples/pendulum/Pendulum.urdf")) def testSingleTimeKinematicConstraint(self): # Demonstrate that a subclass of SingleTimeKinematicConstraint # inherits functional `eval` and `bounds` methods. q = np.zeros(self.r.get_num_positions()) kinsol = self.r.doKinematics(q) ub = np.array([0.0, 0.0, -0.45]) lb = np.array([0.0, 0.0, -0.55]) # This point, at the zero configuration, is # at [0, 0, -1] in world frame as well. body_pt = np.array([0., 0., -1]) constraint = ik.WorldPositionConstraint( self.r, self.r.FindBodyIndex("arm"), body_pt, lb, ub) self.assertIsInstance(constraint, ik.SingleTimeKinematicConstraint) lb_inspect, ub_inspect = constraint.bounds(t=0.) self.assertTrue(np.allclose(lb_inspect, lb)) self.assertTrue(np.allclose(ub_inspect, ub)) c, dc_dq = constraint.eval(0., kinsol) # Because the floating base position is all 0's and the arm # origin is not offset from the floating base, the world # point should equal the body point. c_expected = body_pt dc_dq_expected = np.array([[1., 0., 0., 0., -1., 0., -1.], [0., 1., 0., 1., 0., 0., 0.], [0., 0., 1., 0., 0., 0., 0.]]) self.assertTrue(np.allclose(c, c_expected)) self.assertTrue(np.allclose(dc_dq, dc_dq_expected)) def testPostureConstraint(self): q = -0.9 posture_constraint = ik.PostureConstraint(self.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(self.r) results = ik.InverseKin( self.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(self.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(self.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 testWorldGazeTargetConstraint(self): model = self.r body = 2 axis = np.ones([3, 1]) target = np.ones([3, 1]) gaze_origin = np.zeros([3, 1]) cone_threshold = 1e-3 tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin, cone_threshold) # Test that construction doesn't fail with a given timespan. ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin, cone_threshold, tspan) def testRelativePositionConstraint(self): model = self.r pts = np.zeros([3, 1]) lb = -np.ones([3, 1]) ub = np.ones([3, 1]) bodyA_idx = 1 bodyB_idx = 2 translation = np.zeros(3) quaternion = np.array([1, 0, 0, 0]) bTbp = np.concatenate([translation, quaternion]).reshape(7, 1) tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx, bTbp) # Test that construction doesn't fail with a given timespan. ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx, bTbp, tspan) def testRelativeGazeDirConstraint(self): model = self.r bodyA_idx = 1 bodyB_idx = 2 axis = np.ones([3, 1]) dir = np.ones([3, 1]) conethreshold = 1e-3 tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir, conethreshold) # Test that construction doesn't fail with a given timespan. ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir, conethreshold, tspan) def testMinDistanceConstraint(self): model = self.r min_distance = 1e-2 active_bodies_idx = list() active_group_name = set() tspan = np.array([0., 1.]) # Test that construction doesn't fail with the default timespan. ik.MinDistanceConstraint(model, min_distance, active_bodies_idx, active_group_name) # Test that construction doesn't fail with a given timespan. ik.MinDistanceConstraint(model, min_distance, active_bodies_idx, active_group_name, tspan)