def make_wrench_stamped(force, torque, frame='/body'):
'''Make a WrenchStamped message without all the fuss
Frame defaults to body
'''
wrench = geometry_msgs.WrenchStamped(
header=make_header(frame),
wrench=geometry_msgs.Wrench(force=geometry_msgs.Vector3(*force),
torque=geometry_msgs.Vector3(*torque)))
return wrench
def send_wrench(self, force, torque):
'''
Specify wrench in Newtons, Newton-meters
(Are you supposed to say Newtons-meter? Newtons-Meters?)
'''
wrench_msg = geometry_msgs.WrenchStamped(
header=sub8_utils.make_header(),
wrench=geometry_msgs.Wrench(force=geometry_msgs.Vector3(*force),
torque=geometry_msgs.Vector3(*torque)))
self.wrench_pub.publish(wrench_msg)
def publish_forcetorque(self):
ft_w = self.domain.forcetorque_measurement
f_w = ft_w[0:2]
# this is ft measured in the physics simulation frame.
# however, in real life, the sensor is mounted on the grasp frame.
# so project the force into the rotated frame.
a = self.domain.dynamics.grasp_body.angle
m = tf.transformations.rotation_matrix(-a, [0, 0, 1])[0:2, 0:2]
f_g = np.dot(m, f_w)
f = geom_msg.Vector3(f_g[0], f_g[1], 0)
t = geom_msg.Vector3(0, 0, ft_w[2])
ws = geom_msg.WrenchStamped(
std_msg.Header(None, self.get_domain_sim_time(), "grasp"),
geom_msg.Wrench(f, t))
self.forcetorque_publisher.publish(ws)
def numpy_to_wrench(forcetorque):
return geometry_msgs.Wrench(
force=geometry_msgs.Vector3(*forcetorque[:3]),
torque=geometry_msgs.Vector3(*forcetorque[3:])
)
def wrench_kdl_to_msg(w):
return geometry_msgs.Wrench(force=geometry_msgs.Vector3(*w.force),
torque=geometry_msgs.Vector3(*w.torque))
def publishFakeWrench(self):
self.state_wrench_pub.publish(
geometry_msgs.WrenchStamped(
header=std_msgs.Header(frame_id=self.kinematics.tip_link),
wrench=geometry_msgs.Wrench(
force=geometry_msgs.Vector3(x=0.0, y=0.0, z=0.0))))
