def trans_wrench_cb(self, ws_old): ws_new = WrenchStamped() ws_new.header = copy.deepcopy(ws_old.header) ws_new.header.frame_id = self.target_frame vec3_stamped = Vector3Stamped() vec3_stamped.header = ws_old.header try: self.tf_list.waitForTransform(self.target_frame, ws_old.header.frame_id, ws_old.header.stamp, rospy.Duration(1.)) except Exception as e: print e rospy.logwarn( "Timeout waiting for transform from %s to target frame %s" % (ws_old.header.frame_id, self.target_frame)) return vec3_stamped.vector = ws_old.wrench.force ws_new.wrench.force = self.tf_list.transformVector3( self.target_frame, vec3_stamped).vector vec3_stamped.vector = ws_old.wrench.torque ws_new.wrench.torque = self.tf_list.transformVector3( self.target_frame, vec3_stamped).vector self.ws_pub.publish(ws_new)
def callback(self, msg): #rospy.loginfo(rospy.get_name() + ": I heard %s" % str(msg)) self.tfl.waitForTransform("base_link", "l_force_torque_link", msg.header.stamp, rospy.Duration(1.0)) trans, rot = self.tfl.lookupTransform("base_link", "l_force_torque_link", msg.header.stamp) mat = transformations.quaternion_matrix(rot) R = mat[:3, :3] / mat[3, 3] f_in = np.array( [msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z]) t_in = np.array( [msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z]) f_est = self.m * self.g * R[2, :] t_est = self.m * self.g * self.d * np.array([0, R[2, 2], -R[2, 1]]) #print "force est: ", f_est #print "torque est: ", t_est f_out = f_in - f_est t_out = t_in - t_est out = WrenchStamped() out.header = msg.header out.wrench.force = Vector3(*f_out) out.wrench.torque = Vector3(*t_out) self.pub.publish(out)
def sensor_cb(self, msg): filtered_msg = WrenchStamped() filtered_msg.header = msg.header self.fx.append(msg.wrench.force.x) self.fy.append(msg.wrench.force.y) self.fz.append(msg.wrench.force.z) self.tx.append(msg.wrench.torque.x) self.ty.append(msg.wrench.torque.y) self.tz.append(msg.wrench.torque.z) if len(self.fx) < self.window_size: return fx = lfilter(self.b, self.a, self.fx) fy = lfilter(self.b, self.a, self.fy) fz = lfilter(self.b, self.a, self.fz) tx = lfilter(self.b, self.a, self.tx) ty = lfilter(self.b, self.a, self.ty) tz = lfilter(self.b, self.a, self.tz) f_filtered = np.array([fx[-1], fy[-1], fz[-1]]) t_filtered = np.array([tx[-1], ty[-1], tz[-1]]) filtered_msg.wrench.force = Vector3(*f_filtered) filtered_msg.wrench.torque = Vector3(*t_filtered) try: self.filtered_pub.publish(filtered_msg) except: pass
def resend_data(msg): global count global Fx, Fy, Fz, Tx, Ty, Tz global mean_pub,dev_pub global publish_each # filtering val_fx = Fx.add(msg.wrench.force.x) val_fy = Fy.add(msg.wrench.force.y) val_fz = Fz.add(msg.wrench.force.z) val_tx = Tx.add(msg.wrench.torque.x) val_ty = Ty.add(msg.wrench.torque.y) val_tz = Tz.add(msg.wrench.torque.z) if count == publish_each: mean_wr = WrenchStamped() dev_wr = WrenchStamped() # copying acquired msg header to new msg mean_wr.header = msg.header dev_wr.header = msg.header # copying force data X mean_wr.wrench.force.x = val_fx[0] dev_wr.wrench.force.x = val_fx[1] # copying force data Y mean_wr.wrench.force.y = val_fy[0] dev_wr.wrench.force.y = val_fy[1] # copying force data Z mean_wr.wrench.force.z = val_fz[0] dev_wr.wrench.force.z = val_fz[1] # copying torque data X mean_wr.wrench.torque.x = val_tx[0] dev_wr.wrench.torque.x = val_tx[1] # copying torque data Y mean_wr.wrench.torque.y = val_ty[0] dev_wr.wrench.torque.y = val_ty[1] # copying torque data Z mean_wr.wrench.torque.z = val_tz[0] dev_wr.wrench.torque.z = val_tz[1] # publishing mean_pub.publish(mean_wr) dev_pub.publish(dev_wr) real_pub.publish(msg) count = -1 count += 1
def do_transform_wrench(wrench, transform): force = Vector3Stamped() torque = Vector3Stamped() force.vector = wrench.wrench.force torque.vector = wrench.wrench.torque res = WrenchStamped() res.wrench.force = do_transform_vector3(force, transform).vector res.wrench.torque = do_transform_vector3(torque, transform).vector res.header = transform.header return res
def callback_ft(data): global pub_ft global listener wrench = WrenchStamped() wrench.wrench.force.x = data.wrench.force.x wrench.wrench.force.y = data.wrench.force.y wrench.wrench.force.z = data.wrench.force.z wrench.wrench.torque.x = data.wrench.torque.x wrench.wrench.torque.y = data.wrench.torque.y wrench.wrench.torque.z = data.wrench.torque.z wrench.header = data.header pub_ft.publish(wrench)
def main(): global model, initialising_ft, offset_ft_data model = BumperModel() pub_compliance_svr = rospy.Publisher('qolo/compliance/svr', WrenchStamped, queue_size=1) dat_compliance_svr = WrenchStamped() # ---- Starting ROS Node ---- rospy.init_node('bumper_prediction', anonymous=True) rate = rospy.Rate(200) # ---- Suscribe to raw ft sensor ---- ftsub = rospy.Subscriber("/rokubi_node_front/ft_sensor_measurements", WrenchStamped, ft_sensor_callback, queue_size=1) initialising_ft = True rospy.loginfo('Waiting for FT Sensor Offset: 5 sec') time.sleep(5) initialising_ft = False offset_ft_data = np.array([ np.mean(init_ft_data['Fx']), np.mean(init_ft_data['Fy']), np.mean(init_ft_data['Fz']), np.mean(init_ft_data['Mx']), np.mean(init_ft_data['My']), np.mean(init_ft_data['Mz']), ]) print(colored(":" * 80, "green")) print(colored("Bumper Prediction Initialization... done", "green")) print(colored(":" * 80, "green")) while not rospy.is_shutdown(): svr_data = model.predict(np.reshape(ft_data, (1, -1))) dat_compliance_svr.header = make_header("tf_ft_front") dat_compliance_svr.wrench.force.x = svr_data[0] dat_compliance_svr.wrench.force.y = svr_data[1] dat_compliance_svr.wrench.torque.z = svr_data[2] pub_compliance_svr.publish(dat_compliance_svr) logger.log('bumper_prediction', *svr_data) rate.sleep()
def timer_callback(self): if not self.hal_ft_sensor.is_calibrated: print('FT Not Calibrated') else: msg = WrenchStamped() msg.header = Header() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = self.ft_link forces_torques = self.hal_ft_sensor.get_force_torque() msg.wrench.force.x = float(forces_torques[0]) msg.wrench.force.y = float(forces_torques[1]) msg.wrench.force.z = float(forces_torques[2]) msg.wrench.torque.x = float(forces_torques[3]) msg.wrench.torque.y = float(forces_torques[4]) msg.wrench.torque.z = float(forces_torques[5]) self.publisher_.publish(msg)
def cb_raw_force(self, msg): if self._reset_time > rospy.Time.now(): self._zero_offset_buffer.append(msg) else: if self._resetting: rospy.logwarn("reset!") self._zero_offset = self._avg_wrench(self._zero_offset_buffer) self._resetting = False ws = WrenchStamped() ws.header = msg.header ws.wrench.force.x = msg.wrench.force.x - self._zero_offset.force.x ws.wrench.force.y = msg.wrench.force.y - self._zero_offset.force.y ws.wrench.force.z = msg.wrench.force.z - self._zero_offset.force.z ws.wrench.torque.x = msg.wrench.torque.x - self._zero_offset.torque.x ws.wrench.torque.y = msg.wrench.torque.y - self._zero_offset.torque.y ws.wrench.torque.z = msg.wrench.torque.z - self._zero_offset.torque.z self.wrench = ws.wrench self._pub.publish(ws)
def resend_data(msg): global count global Fx, Fy, Fz, Tx, Ty, Tz global mean_pub,dev_pub global publish_each global orig_f,mean_f,std_f # filtering val_fx = Fx.add(msg.wrench.force.x) val_fy = Fy.add(msg.wrench.force.y) val_fz = Fz.add(msg.wrench.force.z) val_tx = Tx.add(msg.wrench.torque.x) val_ty = Ty.add(msg.wrench.torque.y) val_tz = Tz.add(msg.wrench.torque.z) # writing data to files orig_f.write("%15.5f" % msg.header.stamp.to_sec() + ";") mean_f.write("%15.5f" % msg.header.stamp.to_sec() + ";") std_f.write("%15.5f" % msg.header.stamp.to_sec() + ";") orig_f.write(str(msg.wrench.force.x) + ";") orig_f.write(str(msg.wrench.force.y) + ";") orig_f.write(str(msg.wrench.force.z) + ";") orig_f.write(str(msg.wrench.torque.x) + ";") orig_f.write(str(msg.wrench.torque.y) + ";") orig_f.write(str(msg.wrench.torque.z) + ";\n") mean_f.write(str(val_fx[0]) + ";") mean_f.write(str(val_fy[0]) + ";") mean_f.write(str(val_fz[0]) + ";") mean_f.write(str(val_tx[0]) + ";") mean_f.write(str(val_ty[0]) + ";") mean_f.write(str(val_tz[0]) + ";\n") std_f.write(str(val_fx[1]) + ";") std_f.write(str(val_fy[1]) + ";") std_f.write(str(val_fz[1]) + ";") std_f.write(str(val_tx[1]) + ";") std_f.write(str(val_ty[1]) + ";") std_f.write(str(val_tz[1]) + ";\n") if count == publish_each: mean_wr = WrenchStamped() dev_wr = WrenchStamped() # copying acquired msg header to new msg mean_wr.header = msg.header dev_wr.header = msg.header # copying force data X mean_wr.wrench.force.x = val_fx[0] dev_wr.wrench.force.x = val_fx[1] # copying force data Y mean_wr.wrench.force.y = val_fy[0] dev_wr.wrench.force.y = val_fy[1] # copying force data Z mean_wr.wrench.force.z = val_fz[0] dev_wr.wrench.force.z = val_fz[1] # copying torque data X mean_wr.wrench.torque.x = val_tx[0] dev_wr.wrench.torque.x = val_tx[1] # copying torque data Y mean_wr.wrench.torque.y = val_ty[0] dev_wr.wrench.torque.y = val_ty[1] # copying torque data Z mean_wr.wrench.torque.z = val_tz[0] dev_wr.wrench.torque.z = val_tz[1] # publishing mean_pub.publish(mean_wr) dev_pub.publish(dev_wr) real_pub.publish(msg) count = -1 count += 1
def array_to_wrench_stamped(header, array): msg = WrenchStamped() msg.header = header msg.wrench = array_to_wrench(array) return msg