def test_cart_controller_base_bot(self):
r = PointyBot(1, urdf='2d_base_bot.urdf')
c = CartesianController(r)
start = np.array([0, -1., 0])
start_dict = {
'joint_x': start[0],
'joint_y': start[1],
'rot_z': start[2]
}
# goal = {'eef': [0., 0., 0., 1, -1, 1.1, 0]}
# goal = {'eef': [0., 0., -0.09983342, 0.99500417, -1, 1.1, 0]}
goal = {'eef': [0., 0., 0.52268723, 0.85252452, -1, 1.1, 0]}
r.set_joint_state(start_dict)
c.set_goal(goal)
for i in range(20):
cmd_dict = c.get_next_command()
self.assertIsNotNone(cmd_dict)
next_state = OrderedDict()
robot_state = r.get_state()
for j, (joint_name, joint_change) in enumerate(cmd_dict.items()):
next_state[joint_name] = robot_state[joint_name] + joint_change
r.set_joint_state(next_state)
print('iteration #{}: {}'.format(i + 1,
r.get_eef_position2()['eef']))
np.testing.assert_array_almost_equal(r.get_eef_position2()['eef'],
goal['eef'],
decimal=4)
def test_eef_controller_base_bot(self):
r = PointyBot(1, urdf='2d_base_bot.urdf')
c = EEFPositionControl(r)
start = np.array([0, -1., 0])
start_dict = {
'joint_x': start[0],
'joint_y': start[1],
'rot_z': start[2]
}
goal = {'eef': [1, 1, 0]}
r.set_joint_state(start_dict)
c.set_goal(goal)
for i in range(30):
cmd_dict = c.get_next_command()
self.assertIsNotNone(cmd_dict)
next_state = OrderedDict()
robot_state = r.get_state()
for j, (joint_name, joint_change) in enumerate(cmd_dict.items()):
next_state[joint_name] = robot_state[joint_name] + joint_change
r.set_joint_state(next_state)
print('iteration #{}: {}'.format(i + 1,
r.get_eef_position2()['eef'][4:]))
np.testing.assert_array_almost_equal(r.get_eef_position2()['eef'][4:],
goal['eef'])