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)
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"))
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") )