def test_joint_limits(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) assert_raises(KeyError, tm.get_joint_limits, "unknown_joint") assert_array_almost_equal(tm.get_joint_limits("joint1"), (-1, 1)) assert_array_almost_equal(tm.get_joint_limits("joint2"), (-1, np.inf)) assert_array_almost_equal(tm.get_joint_limits("joint3"), (-np.inf, 1))
from pytransform.urdf import UrdfTransformManager # A script that helps to determine the optimum ratio # between step-sizes in Cartesian and joint space. # We sample joint angles with variance 1 and measure # the resulting standard deviation in Cartesian space. tm = UrdfTransformManager() tm.load_urdf(open("../data/kuka_lbr.urdf", "r")) random_state = np.random.RandomState(42) n_samples = 1000 joint_limits = np.array( [tm.get_joint_limits("kuka_lbr_l_joint_%d" % (j + 1)) for j in range(7)] ) #joint_angles = (random_state.rand(n_samples, 7) * # (joint_limits[:, 1] - joint_limits[:, 0]) + # joint_limits[:, 0]) joint_angles = random_state.randn(n_samples, 7) positions = np.empty((n_samples, 3)) for n in range(n_samples): for j in range(7): tm.set_joint("kuka_lbr_l_joint_%d" % (j + 1), joint_angles[n, j]) positions[n] = tm.get_transform("kuka_lbr_l_link_7", "kuka_lbr_l_link_0")[:3, 3] print(np.std(joint_angles, axis=0)) print(np.mean(positions, axis=0)) print(np.std(positions, axis=0))