Exemplo n.º 1
0
    def test_make_wrench_stamped(self):
        '''Test that wrenchmaking works'''
        for k in range(10):
            force = np.random.random(3) * 10
            torque = np.random.random(3) * 10
        wrench_msg = make_wrench_stamped(force, torque, frame='/enu')

        msg_force = rosmsg_to_numpy(wrench_msg.wrench.force)  # noqa
        msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque)  # noqa
        self.assertIsNotNone(msg_force)
        self.assertIsnotNone(msg_torque)
Exemplo n.º 2
0
    def request_wrench_cb(self, msg):
        '''Callback for requesting a wrench'''
        time_now = rospy.Time.now()
        if (time_now - self.last_command_time) < self._min_command_time:
            return
        else:
            self.last_command_time = rospy.Time.now()

        force = rosmsg_to_numpy(msg.wrench.force)
        torque = rosmsg_to_numpy(msg.wrench.torque)
        wrench = np.hstack([force, torque])

        success = False
        while not success:
            u, success = self.map(wrench)
            if not success:
                # If we fail to compute, shrink the wrench
                wrench = wrench * 0.75
                continue

            thrust_cmds = []
            # Assemble the list of thrust commands to send
            for name, thrust in zip(self.thruster_name_map, u):
                # > Can speed this up by avoiding appends
                if name in self.dropped_thrusters:
                    continue  # Ignore dropped thrusters

                if np.fabs(thrust) < self.min_commandable_thrust:
                    thrust = 0
                thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))

        # TODO: Make this account for dropped thrusters

        actual_wrench = self.B.dot(u)
        self.actual_wrench_pub.publish(
            msg_helpers.make_wrench_stamped(actual_wrench[:3],
                                            actual_wrench[3:]))
        self.thruster_pub.publish(thrust_cmds)
Exemplo n.º 3
0
    def request_wrench_cb(self, msg):
        '''Callback for requesting a wrench'''
        time_now = rospy.Time.now()
        if (time_now - self.last_command_time) < self._min_command_time:
            return
        else:
            self.last_command_time = rospy.Time.now()

        force = rosmsg_to_numpy(msg.wrench.force)
        torque = rosmsg_to_numpy(msg.wrench.torque)
        wrench = np.hstack([force, torque])

        success = False
        while not success:
            u, success = self.map(wrench)
            if not success:
                # If we fail to compute, shrink the wrench
                wrench = wrench * 0.75
                continue

            thrust_cmds = []
            # Assemble the list of thrust commands to send
            for name, thrust in zip(self.thruster_name_map, u):
                # > Can speed this up by avoiding appends
                if name in self.dropped_thrusters:
                    continue  # Ignore dropped thrusters

                if np.fabs(thrust) < self.min_commandable_thrust:
                    thrust = 0
                thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))

        # TODO: Make this account for dropped thrusters

        actual_wrench = self.B.dot(u)
        self.actual_wrench_pub.publish(
            msg_helpers.make_wrench_stamped(actual_wrench[:3], actual_wrench[3:])
        )
        self.thruster_pub.publish(thrust_cmds)
Exemplo n.º 4
0
    def turbuloop(self):
        '''
        The idea is to create a smooth application of force to emulate underwater motion.
        The Turbuloop applies a wrench with a magnitude that varies like a squared function with zeros on both sides
            so that there are no sudden changes in the force.
        '''

        model_name = 'sub8::base_link'

        # Used to gently apply a force on the sub, time_step times per 1 / freq
        time_step = 5.0

        while not rospy.is_shutdown():
            turbulence_mag_step = self.turbulence_mag / time_step
            sleep_step = 1 / (self.turbulence_freq * time_step)

            # Random unit vector scaled by the desired magnitude
            f = np.random.uniform(-1, 1, size=3) * self.turbulence_mag
            r = np.random.uniform(-1, 1, size=3)
            r[:2] = 0  # C3 doesn't like variation in x or y rotation :(

            for i in range(int(time_step)):
                # Square function: -(x - a/2)^2 + (a/2)^2
                mag_multiplier = -((i - time_step / 2) ** 2 - (time_step / 2) ** 2 - 1) * turbulence_mag_step

                # Create service call
                body_wrench = ApplyBodyWrenchRequest()
                body_wrench.body_name = model_name
                body_wrench.reference_frame = model_name
                body_wrench.wrench = msg_helpers.make_wrench_stamped(f * mag_multiplier, r * mag_multiplier).wrench
                body_wrench.start_time = rospy.Time()
                body_wrench.duration = rospy.Duration(sleep_step)

                #rospy.loginfo("{}: Wrench Applied: {}".format(i, body_wrench.wrench))

                self.set_wrench(body_wrench)

                rospy.sleep(sleep_step)