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