Esempio 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)
        msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque)
Esempio n. 2
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)
        msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque)
Esempio n. 3
0
    def twist_cb(self, msg):
        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
        angular = sub8_utils.rosmsg_to_numpy(msg.angular)

        if self.behavior == 'wrench':
            # Directly transcribe linear and angular 'velocities' to torque
            # TODO: Setup actual TF!

            self.wrench_pub.publish(
                sub8_utils.make_wrench_stamped(
                    force=self.linear_gain * self.world_to_body.dot(linear),
                    torque=self.angular_gain * self.world_to_body.dot(angular)
                )
            )
        else:
            raise(NotImplementedError("Velocity and position control by mouse are not yet supported"))
Esempio n. 4
0
    def twist_cb(self, msg):
        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
        angular = sub8_utils.rosmsg_to_numpy(msg.angular)

        if self.behavior == 'wrench':
            # Directly transcribe linear and angular 'velocities' to torque
            # TODO: Setup actual TF!

            self.wrench_pub.publish(
                sub8_utils.make_wrench_stamped(
                    force=self.linear_gain * self.world_to_body.dot(linear),
                    torque=self.angular_gain *
                    self.world_to_body.dot(angular)))
        else:
            raise (NotImplementedError(
                "Velocity and position control by mouse are not yet supported")
                   )