def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(self._position_gain * linear)
self.diff_position = np.subtract(self.cur_position, self.target_position)
if self.mode == '2d':
# Ignore 6-dof shenanigans
angular[0] = 0
angular[1] = 0
gained_angular = self._orientation_gain * angular
skewed = sub8_utils.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
self.target_distance = round(np.linalg.norm(np.array([self.diff_position[0], self.diff_position[1]])), 3)
self.target_depth = round(self.diff_position[2], 3)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank)
self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
def test_skew_symmetric_cross(self):
'''Test that the skew symmetric cross product matrix produces the definition
[1] https://en.wikipedia.org/wiki/Cross_product#Skew-symmetric_matrix
'''
skew_sym = skew_symmetric_cross([1, 2, 3])
truth = np.array([
[+0, -3, +2],
[+3, +0, -1],
[-2, +1, +0],
])
np.testing.assert_allclose(skew_sym, truth, err_msg="Did not make a Skew-symmetric matrix. Pretty big screw-up imho.")
def test_skew_symmetric_cross(self):
'''Test that the skew symmetric cross product matrix produces the definition
[1] https://en.wikipedia.org/wiki/Cross_product#Skew-symmetric_matrix
'''
skew_sym = skew_symmetric_cross([1, 2, 3])
truth = np.array([
[+0, -3, +2],
[+3, +0, -1],
[-2, +1, +0],
])
np.testing.assert_allclose(
skew_sym,
truth,
err_msg=
"Did not make a Skew-symmetric matrix. Pretty big screw-up imho.")
def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(self._position_gain * linear)
gained_angular = self._orientation_gain * angular
skewed = sub8_utils.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank)
self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(
self._position_gain * linear)
gained_angular = self._orientation_gain * angular
skewed = sub8_utils.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(
blank)
self.publish_target_pose(self.target_position,
self.target_orientation_quaternion)
