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_joint_controller_pr2(self):
t = time()
r = self.default_pr2()
c = JointSpaceControl(r, weight=1)
joints = [
'torso_lift_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint',
'l_shoulder_lift_joint', 'l_shoulder_pan_joint',
'l_upper_arm_roll_joint', 'l_wrist_flex_joint',
'l_wrist_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint',
'r_shoulder_lift_joint', 'r_shoulder_pan_joint',
'r_upper_arm_roll_joint', 'r_wrist_flex_joint',
'r_wrist_roll_joint'
]
goal = [0.0] * len(joints)
goal_dict = {joint: goal[i] for i, joint in enumerate(joints)}
c.set_goal(goal_dict)
print('time spent on init: {}'.format(time() - t))
ts = []
for i in range(40):
t = time()
cmd_dict = c.get_next_command()
ts.append(time() - t)
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('time spent per get_next_command: {}'.format(np.mean(ts)))
for k, v in goal_dict.items():
self.assertAlmostEqual(v, r.get_state()[k], places=5)
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_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)