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