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)