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