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
