def test_qpbuilder_1(self):
big = 1e9
robot_weight = .9
r = PointyBot(robot_weight)
control_weight = 42
c = JointSpaceControl(r, control_weight)
qpbuilder = c.qp_problem_builder
start = np.array([.5, 1.05, .35])
start_dict = {
'joint_x': start[0],
'joint_y': start[1],
'joint_z': start[2]
}
goal_array = np.array([.7, .8, .9])
goal = {
'joint_x': goal_array[0],
'joint_y': goal_array[1],
'joint_z': goal_array[2]
}
r._update_observables(start_dict)
c.set_goal(goal)
c.get_next_command()
A = qpbuilder.np_A
expected_A = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0], [1, 0, 0, 1, 0, 0],
[0, 1, 0, 0, 1, 0], [0, 0, 1, 0, 0, 1]])
np.testing.assert_array_almost_equal(A, expected_A)
H = qpbuilder.np_H
expected_H = np.identity(6) * np.array([
robot_weight, robot_weight, robot_weight, control_weight,
control_weight, control_weight
])
np.testing.assert_array_almost_equal(H, expected_H)
lb = qpbuilder.np_lb
expected_lb = np.array([-0.1, -.3, -.5, -big, -big, -big])
np.testing.assert_array_equal(lb, expected_lb)
ub = qpbuilder.np_ub
expected_ub = np.array([0.1, .3, .5, big, big, big])
np.testing.assert_array_equal(ub, expected_ub)
lbA = qpbuilder.np_lbA
expected_lbA = np.array([-1.1, -1.3, -1.5])
expected_lbA = np.concatenate(
(expected_lbA - start, goal_array - start))
np.testing.assert_array_almost_equal(lbA, expected_lbA)
ubA = qpbuilder.np_ubA
expected_ubA = np.array([1.2, 1.4, 1.6])
expected_ubA = np.concatenate(
(expected_ubA - start, goal_array - start))
np.testing.assert_array_almost_equal(ubA, expected_ubA)
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_position()['eef'][:3, 3]))
np.testing.assert_array_almost_equal(
r.get_eef_position()['eef'][:3, 3], goal['eef'])
def test_jointcontroller_2(self):
r = PointyBot(weight=.01, urdf='2d_base_bot.urdf')
c = JointSpaceControl(r, weight=1)
start = np.array([0, -1., .5])
start_dict = {
'joint_x': start[0],
'joint_y': start[1],
'rot_z': start[2]
}
goal = np.array([1, 1, -1])
goal_dict = {'joint_x': goal[0], 'joint_y': goal[1], 'rot_z': goal[2]}
r.set_joint_state(start_dict)
c.set_goal(goal_dict)
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))
for k, v in goal_dict.items():
self.assertAlmostEqual(v, r.get_state()[k])
def test_jointcontroller_1(self):
r = PointyBot(.9)
c = JointSpaceControl(r, 42)
start = np.array([.5, 1.05, .35])
start_dict = {
'joint_x': start[0],
'joint_y': start[1],
'joint_z': start[2]
}
goal = np.array([.7, .8, .9])
goal_dict = {
'joint_x': goal[0],
'joint_y': goal[1],
'joint_z': goal[2]
}
r.set_joint_state(start_dict)
c.set_goal(goal_dict)
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))
for k, v in goal_dict.items():
self.assertAlmostEqual(v, r.get_state()[k])
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_base(self):
r_kdl = KDL('2d_base_bot.urdf', 'base_link', 'eef')
r = PointyBot(1, urdf='2d_base_bot.urdf')
head = OrderedDict([('joint_x', 0), ('joint_y', 0), ('rot_z', 0)])
js = [0.] * 3
self.set_js(r, head, js)
eef_pose = r.get_eef_position()
np.testing.assert_array_almost_equal(eef_pose['eef'], r_kdl.fk(js))
js = [0.1] * 3
self.set_js(r, head, js)
eef_pose = r.get_eef_position()
np.testing.assert_array_almost_equal(eef_pose['eef'], r_kdl.fk(js))
js = [-0.2] * 3
self.set_js(r, head, js)
eef_pose = r.get_eef_position()
np.testing.assert_array_almost_equal(eef_pose['eef'], r_kdl.fk(js))
js = [(.3 if x % 2 == 0 else -.1) for x in range(3)]
self.set_js(r, head, js)
eef_pose = r.get_eef_position()
np.testing.assert_array_almost_equal(eef_pose['eef'], r_kdl.fk(js))