def make_pose_stamped(position, orientation, frame='/body'):
wrench = geometry_msgs.WrenchStamped(
header=make_header(frame),
pose=geometry_msgs.Pose(
force=geometry_msgs.Vector3(*position),
orientation=geometry_msgs.Quaternion(*orientation)))
return wrench
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node("test")
test_msg = geom.WrenchStamped()
node.get_logger().info(
yaml.dump(YamlWrenchStamped(test_msg), sort_keys=False))
node.destroy_node()
rclpy.shutdown()
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 wrench_cb(msg):
global tare_offset
time = rospy.Time(0) # alternative: msg.header.stamp
force_frame = msg.header.frame_id
# get transforms: gravity -> com -> sensor
try:
tf_gravity = get_frame(com_frame, gravity_frame, time)
tf_com = get_frame(force_frame, com_frame, time)
except tf2.TransformException as e:
rospy.logerr(e)
init_transform(com_frame, gravity_frame)
init_transform(force_frame, com_frame)
return
# compute gravity at com: change coordinate system (rotate)
gravity_at_com = tf_gravity.M * gravity
# compute gravity at sensor: screw theory transform
# this covers coordinate change and translation-lever
gravity_at_sensor = tf_com * gravity_at_com
# compensate
compensated = wrench_msg_to_kdl(msg) - gravity_at_sensor
wrench_buffer.append(compensated)
# Tare
if run_tare.is_set():
tare_offset = mean_wrench(wrench_buffer)
run_tare.clear()
rospy.loginfo("Tared sensor")
compensated = compensated - tare_offset
# publish
compensated_msg = geometry_msgs.WrenchStamped(
header=msg.header, wrench=wrench_kdl_to_msg(compensated))
pub.publish(compensated_msg)
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))))
