def setUpClass(cls): cls.robot_model = Model( "franka", os.path.join(os.path.dirname(os.path.realpath(__file__)), "panda_arm.urdf")) joint_state = JointState(cls.robot_model.get_robot_name(), cls.robot_model.get_number_of_joints()) joint_state.set_positions([ -1.957518, 1.037530, -1.093933, -1.485144, -1.937432, 2.251972, -1.373487 ]) joint_state.set_velocities([ 0.308158, 0.378429, 0.496303, -0.098917, -0.832357, -0.542046, 0.826675 ]) joint_state.set_accelerations([ -0.695244, 0.651634, 0.076685, 0.992269, -0.843649, -0.114643, -0.786694 ]) cls.joint_state = joint_state cls.test_gravity_expects = [ -0.000000, -36.985759, -18.785455, 11.384067, -0.532842, -1.346848, -0.071370 ] cls.test_coriolis_expects = [ -0.049325, -0.064489, -0.060717, -0.295963, -0.009998, 0.026109, 0.003887 ] cls.test_inertia_expects = [ -0.733398, 0.568876, -0.066627, -0.156614, -0.030068, -0.025849, -0.006774 ] cls.tol = 1e-5
def test_clamp_in_range(self): joint_state = JointState("robot", self.robot_model.get_joint_frames()) joint_state.set_positions([ -0.059943, 1.667088, 1.439900, -1000, -1.164922, 0.948034, 1.292717 ]) joint_state.set_velocities([ -0.059943, 31.667088, 1.439900, -1.367141, -1.164922, 0.948034, 2.239983 ]) joint_state.set_torques([ -0.329909, -0.235174, -1.881858, -922.491807, 0.674615, 0.996670, 0.345810 ]) joint_positions = JointPositions(joint_state) joint_velocities = JointVelocities(joint_state) joint_torques = JointTorques(joint_state) self.assertFalse(self.robot_model.in_range(joint_positions)) self.assertFalse(self.robot_model.in_range(joint_velocities)) self.assertFalse(self.robot_model.in_range(joint_torques)) self.assertFalse(self.robot_model.in_range(joint_state)) self.assertTrue( self.robot_model.in_range( self.robot_model.clamp_in_range(joint_positions))) self.assertTrue( self.robot_model.in_range( self.robot_model.clamp_in_range(joint_velocities))) self.assertTrue( self.robot_model.in_range( self.robot_model.clamp_in_range(joint_torques))) self.assertTrue( self.robot_model.in_range( self.robot_model.clamp_in_range(joint_state)))
def test_inverse_velocity_constraints(self): eef_frame = self.robot_model.get_frames()[-1] parameters = QPInverseVelocityParameters() parameters.linear_velocity_limit = 0.1 parameters.angular_velocity_limit = 0.2 des_ee_twist = CartesianTwist(eef_frame, [1, 0, 0], [1, 0, 0], self.robot_model.get_base_frame()) joint_velocities = self.robot_model.inverse_velocity( des_ee_twist, JointPositions(self.test_config), parameters) state = JointState(self.test_config) state.set_velocities(joint_velocities.data()) act_ee_twist = self.robot_model.forward_velocity(state) self.assertTrue( np.linalg.norm(act_ee_twist.get_linear_velocity()) <= 0.1) self.assertTrue( np.linalg.norm(act_ee_twist.get_angular_velocity()) <= 0.2)
def test_compute_inertia_torques(self): joint_state = JointState().Random("robot", 7) inertia_torques = self.robot_model.compute_inertia_torques(joint_state) self.assertTrue(isinstance(inertia_torques, JointTorques)) self.assertTrue(np.linalg.norm(inertia_torques.data()) > 0) inertia_torques = self.robot_model.compute_inertia_torques( self.joint_state) [ self.assertAlmostEqual(inertia_torques.get_torques()[i], self.test_inertia_expects[i], delta=self.tol) for i in range(7) ]
def test_inverse_velocity(self): eef_frame = self.robot_model.get_frames()[-1] des_ee_twist = CartesianTwist.Random(eef_frame, self.robot_model.get_base_frame()) joint_velocities = self.robot_model.inverse_velocity( des_ee_twist, JointPositions(self.test_config)) state = JointState(self.test_config) state.set_velocities(joint_velocities.data()) act_ee_twist = self.robot_model.forward_velocity(state) self.assertTrue(des_ee_twist.dist(act_ee_twist) < self.tol) joint_velocities2 = self.robot_model.inverse_velocity( des_ee_twist, JointPositions(self.test_config), QPInverseVelocityParameters()) state2 = JointState(self.test_config) state.set_velocities(joint_velocities2.data()) act_ee_twist2 = self.robot_model.forward_velocity(state2)
def setUpClass(cls): cls.robot_model = Model( "franka", os.path.join(os.path.dirname(os.path.realpath(__file__)), "panda_arm.urdf")) joint_state = JointState(cls.robot_model.get_robot_name(), cls.robot_model.get_joint_frames()) cls.joint_state = joint_state test_config = JointState(cls.robot_model.get_robot_name(), cls.robot_model.get_joint_frames()) test_config.set_positions([ -1.957518, 1.037530, -1.093933, -1.485144, -1.937432, 2.251972, -1.373487 ]) test_config.set_velocities([ 0.308158, 0.378429, 0.496303, -0.098917, -0.832357, -0.542046, 0.826675 ]) cls.test_config = test_config cls.test_jacobian_ee_expects = np.array([ 0.275154, -0.005914, 0.127368, -0.041238, 0.003824, 0.018324, 0.000000, -0.693174, -0.014523, -0.347282, -0.422943, 0.026691, -0.034385, 0.000000, -0.000000, -0.516268, -0.463478, 0.313197, -0.006376, -0.132947, 0.000000, -0.000000, 0.926150, -0.324787, -0.254761, -0.935275, -0.138023, -0.842118, 0.000000, -0.377155, -0.797555, 0.591396, 0.050315, -0.963323, 0.236446, 1.000000, 0.000000, 0.508349, 0.765080, -0.350326, 0.230127, 0.484697 ]).reshape(6, 7) cls.test_fk_ee_expects = CartesianPose( "ee", [-0.693174, -0.275154, 0.348681], [0.548764, -0.464146, -0.205476, 0.664234], cls.robot_model.get_base_frame()) cls.test_fk_link4_expects = CartesianPose( "link4", [-0.177776, -0.242212, 0.461029], [0.717822, -0.327979, 0.099446, 0.606030], cls.robot_model.get_base_frame()) cls.test_velocity_fk_expects = CartesianTwist( "ee", [0.136730, -0.353202, -0.379006, 0.371630, 0.078694, 1.052318], cls.robot_model.get_base_frame()) cls.tol = 1e-3
def test_copy(self): state = JointState().Random("test", 3) for state_copy in [copy.copy(state), copy.deepcopy(state)]: self.assertEqual(state.get_name(), state_copy.get_name()) self.assertListEqual(state.get_names(), state_copy.get_names()) self.assert_np_array_equal(state.data(), state_copy.data())
def test_compute_coriolis_matrix(self): joint_state = JointState().Random("robot", 7) coriolis = self.robot_model.compute_coriolis_matrix(joint_state) self.assertTrue(coriolis.shape[0] == 7 and coriolis.shape[1] == 7)