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 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 get_container_origin_xyz(urdf_path, container_dir): tree = RigidBodyTree(urdf_path) # set robot configuration q = np.zeros(7) # arm position doesn't matter here so just use zeros # look up indices of frames base_idx = tree.FindBody('base_link').get_body_index() container_idx = tree.FindBody( os.path.basename(container_dir)).get_body_index() # do kinematics with oint configuration kinsol = tree.doKinematics(q) # find relative transform between frames T = tree.relativeTransform(kinsol, base_idx, container_idx) return T[:3, 3]
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_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 = 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 = mut.RgbdCameraDiscrete.kDefaultPeriod discrete = 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_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_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_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_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_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))) # 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)) # 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() kinsol = tree.doKinematics(q) # - Sanity check sizes. J_default, v_indices_default = tree.geometricJacobian(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) # - 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)
from pydrake.multibody.rigid_body_tree import RigidBodyTree, RigidBodyFrame import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # load rigid body tree tree = RigidBodyTree("/home/dexai/catkin_ws/src/ice_cream_parlor_description/urdf/Dexai_parlor-drake.urdf") # set robot configuration q = np.zeros(7) #following comment was syler's:# arm position doesn't matter here so just use zeros #prep for 3d graphing fig = plt.figure() ax = fig.add_subplot(111, projection='3d') # look up indices of camera frames cam_idx = tree.FindBody("kinect1_rgb_optical_frame").get_body_index() depth_idx = tree.FindBody("kinect1_depth_optical_frame").get_body_index() # do kinematics with iiwa joint configuration kinsol = tree.doKinematics(q) # find relative transform between rgb and depth optical frame T = tree.relativeTransform(kinsol, cam_idx, depth_idx) T_rgb_d = T # print T X = T[:3,3] # extract position from transformation matrix X_rgb_d = X print (X) ax.plot(???)
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))) # 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)) # 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() kinsol = tree.doKinematics(q) # - Sanity check sizes. J_default, v_indices_default = tree.geometricJacobian(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) # - Check QDotToVelocity and VelocityToQDot methods q = tree.getZeroConfiguration() v_real = np.zeros(num_v) q_ad = np.array(map(AutoDiffXd, q)) v_real_ad = np.array(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 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 that drawKinematicTree runs tree.drawKinematicTree( join(os.environ["TEST_TMPDIR"], "test_graph.dot"))