Example #1
0
    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)
Example #2
0
 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.")
Example #3
0
 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.")
Example #4
0
    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)
Example #5
0
    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)