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 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 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 __init__(self, name, robot):
# Init actionserver
self._action_name = name
self.robot = robot
self.force_sensor = WrenchStamped()
self.last_force = WrenchStamped()
self.Touch_tabletop = False
self.target_pose = PoseStamped()
self.target_pose = PoseStamped()
self.target_pose.pose.position.x = 0.95
self.target_pose.pose.position.y = -0.0
self.target_pose.pose.position.z = 0.85
self.target_height = self.target_pose.pose.position.z
self.target_pose.header.frame_id = _ORIGIN_TF
self.target_pose.pose.position.z += HEIGHT_OFFSET_Z
self.as_result = villa_manipulation.msg.ForcePutDownResult()
self.as_result.touched = False
self.force_time = rospy.get_time()
# self.switch = ImpedanceControlSwitch()
# self.switch.inactivate()
self.cli = actionlib.SimpleActionClient('/move_base/move',
MoveBaseAction)
self.cli.wait_for_server()
jointstates_topic = 'hsrb/joint_states'
rospy.Subscriber(jointstates_topic, JointState, self.joint_state_Cb)
force_sensor_topic = 'hsrb/wrist_wrench/raw'
rospy.Subscriber(force_sensor_topic, WrenchStamped,
self.force_sensor_Cb)
self.vel_pub = rospy.Publisher('/hsrb/command_velocity',
geometry_msgs.msg.Twist,
queue_size=10)
while not rospy.is_shutdown():
try:
self.body = self.robot.try_get('whole_body')
self.gripper = self.robot.try_get('gripper')
self.base = self.robot.try_get('omni_base')
rospy.wait_for_service('/viewpoint_controller/stop')
stop_head = rospy.ServiceProxy('/viewpoint_controller/stop',
Empty)
stop_head()
break
except (exceptions.ResourceNotFoundError,
exceptions.RobotConnectionError) as e:
rospy.logerr(
"Failed to obtain resource: {}\nRetrying...".format(e))
self._as = actionlib.SimpleActionServer(
self._action_name,
villa_manipulation.msg.ForcePutDownAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
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 publishWrench(data, hand):
wr = data.estimated_external_wrench
wrench_msg = Wrench()
wrench_msg.force = Vector3(wr.x, wr.y, wr.z)
wrench_msg.torque = Vector3(wr.c, wr.b, wr.a)
wrench_stamped_msg = WrenchStamped()
wrench_stamped_msg.wrench = wrench_msg
wrench_stamped_msg.header.stamp = rospy.Time(0)
wrench_stamped_msg.header.frame_id = hand_to_frame[hand]
hand_wrench_pub[hand].publish(wrench_stamped_msg)
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 run(self):
while not self._isEnd:
if self._isRunning:
self.pub = rospy.Publisher(self._topic, WrenchStamped, queue_size=10)
while self._isRunning:
msg = WrenchStamped()
msg.header.stamp = rospy.Time.now()
self._lock.acquire()
msg.wrench = self._wrench
msg.header.frame_id = self._frameId
self._lock.release()
self.pub.publish(msg)
self._rate.sleep()
rospy.sleep(0.1)
print "Thread ended"
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 callback_B_field(data):
"""Callback triggered by new Message on Magnetic field topic.
Method updates the B-field for the magnetorquer, computes
the new torque based on the field and publishes the torque.
Args:
data (geometry_msgs.msg.Vector3Stamped): Magnetic field message
"""
B_field_vector = np.zeros(3)
B_field_vector[0] = data.vector.x
B_field_vector[1] = data.vector.y
B_field_vector[2] = data.vector.z
torquer.set_B_field(B_field_vector)
torque = torquer.get_torque()
msg = WrenchStamped()
msg.header.stamp = data.header.stamp
msg.wrench.force.x = 0.0
msg.wrench.force.y = 0.0
msg.wrench.force.z = 0.0
msg.wrench.torque.x = torque[0]
msg.wrench.torque.y = torque[1]
msg.wrench.torque.z = torque[2]
pubTorque.publish(msg)
def __init__(self):
self.pose_ref_pub_ = rospy.Publisher(
'/uav/impedance_control/pose_stamped_ref_input',
PoseStamped,
queue_size=1)
self.force_torque_ref_pub = rospy.Publisher(
'/uav/impedance_control/force_torque_ref_input',
WrenchStamped,
queue_size=1)
# Services for requesting trajectory interpolation
rospy.sleep(5.)
self.force_setpoint_flag = [False]
self.pose_setpoint_flag = [False]
self.force_setpoint = [WrenchStamped()]
self.pose_setpoint = [PoseStamped()]
self.pose_setpoint[0].header.stamp = rospy.Time.now()
self.pose_setpoint[0].pose.position.x = 1.00
self.pose_setpoint[0].pose.position.y = 0
self.pose_setpoint[0].pose.position.z = 1
self.pose_setpoint[0].pose.orientation.x = 0
self.pose_setpoint[0].pose.orientation.y = 0
self.pose_setpoint[0].pose.orientation.z = 0
self.pose_setpoint[0].pose.orientation.w = 1
self.force_setpoint[0].wrench.force.x = 1.0
self.ramp_velocity = 0.05
self.up = False
self.time = 30
self.run()
def publish_control_wrench(self, force):
"""Publish the thruster manager control set-point.
> *Input arguments*
* `force` (*type:* `numpy.array`): 6 DoF control
set-point wrench vector
"""
if not self.odom_is_init:
return
# Apply saturation
for i in range(6):
if force[i] < -self._control_saturation:
force[i] = -self._control_saturation
elif force[i] > self._control_saturation:
force[i] = self._control_saturation
if not self.thrusters_only:
surge_speed = self._vehicle_model.vel[0]
self.publish_auv_command(surge_speed, force)
return
force_msg = WrenchStamped()
force_msg.header.stamp = self.get_clock().now().to_msg()
force_msg.header.frame_id = '%s/%s' % (self._namespace, self._vehicle_model.body_frame_id)
force_msg.wrench.force.x = force[0]
force_msg.wrench.force.y = force[1]
force_msg.wrench.force.z = force[2]
force_msg.wrench.torque.x = force[3]
force_msg.wrench.torque.y = force[4]
force_msg.wrench.torque.z = force[5]
self._thrust_pub.publish(force_msg)
def process_wrench(self, msg):
cur_wrench = np.mat([
msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z,
msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z
]).T
try:
(ft_pos,
ft_quat) = self.tf_list.lookupTransform(self.gravity_frame,
self.netft_frame,
rospy.Time(0))
except:
return
rot_mat = np.mat(tf_trans.quaternion_matrix(ft_quat))[:3, :3]
z_grav = self.react_mult * rot_mat.T * np.mat([0, 0, -1.]).T
force_grav = np.mat(np.zeros((6, 1)))
force_grav[:3, 0] = self.mass * g * z_grav
torque_grav = np.mat(np.zeros((6, 1)))
torque_grav[3:, 0] = np.mat(
np.cross(self.com_pos.T.A[0], force_grav[:3, 0].T.A[0])).T
zeroing_wrench = force_grav + torque_grav + self.wrench_zero
zeroed_wrench = self.react_mult * (cur_wrench - zeroing_wrench)
if not self.got_zero:
self.wrench_zero = (cur_wrench - (force_grav + torque_grav))
self.got_zero = True
tf_zeroed_wrench = self.transform_wrench(zeroed_wrench)
if tf_zeroed_wrench is None:
return
zero_msg = WrenchStamped(
msg.header,
Wrench(Vector3(*tf_zeroed_wrench[:3, 0]),
Vector3(*tf_zeroed_wrench[3:, 0])))
self.zero_pub.publish(zero_msg)
self.visualize_wrench(tf_zeroed_wrench)
def set_body_wrench(self, force, torque, duration, starting_time):
ns = rospy.get_namespace().replace('/', '')
body_name = '%s/base_link' % ns
self._body_force = np.array(
[self._body_force[i] + force[i] for i in range(3)])
self._body_torque = np.array(
[self._body_torque[i] + torque[i] for i in range(3)])
self._body_wrench_msg = WrenchStamped()
self._body_wrench_msg.header.stamp = rospy.Time().now()
self._body_wrench_msg.header.frame_id = 'world'
self._body_wrench_msg.wrench.force = Vector3(*self._body_force)
self._body_wrench_msg.wrench.torque = Vector3(*self._body_torque)
success = self._service_cb['wrench'](body_name, 'world',
Point(0, 0, 0),
self._body_wrench_msg.wrench,
rospy.Time(secs=starting_time),
rospy.Duration(duration))
if success:
self._logger.info(
'Body wrench perturbation applied!, body_name=%s, t=%.2f s' %
(body_name, rospy.get_time()))
else:
self._logger.error(
'Failed to apply body wrench!, body_name=%s, t=%.2f s' %
(body_name, rospy.get_time()))
def __init__(self):
"""
Initialize OptoforceDriver object
"""
port = rospy.get_param("~port", "/dev/ttyACM0")
sensor_type = rospy.get_param("~type", "m-ch/3-axis")
starting_index = rospy.get_param("~starting_index", 0)
scaling_factors = rospy.get_param("~scale")
# Initialize optoforce driver
try:
self._driver = optoforce.OptoforceDriver(port,
sensor_type,
scaling_factors,
starting_index)
except serial.SerialException as e:
rospy.logfatal("Cannot connect to the sensor " + port
+ (e.message if e.message else ''))
rospy.signal_shutdown("Serial connection failure")
raise
# Create and advertise publishers for each connected sensor
self._publishers = []
self._wrenches = []
topic_basename = "optoforce_"
for i in range(self._driver.nb_sensors()):
self._publishers.append(rospy.Publisher(topic_basename +
str(starting_index + i),
WrenchStamped,
queue_size=100))
wrench = WrenchStamped()
wrench.header.frame_id = topic_basename + str(starting_index + i)
self._wrenches.append(wrench)
def reset(self):
'''
Used to reset the state of the controller.
Sometimes when it disconnects then comes back online, the settings are all
out of wack.
'''
self.last_change_mode = False
self.last_station_hold_state = False
self.last_kill = False
self.last_rc = False
self.last_auto = False
self.start_count = 0
self.last_joy = None
self.active = False
self.killed = False
# self.docking = False
wrench = WrenchStamped()
wrench.header.frame_id = "/base_link"
wrench.wrench.force.x = 0
wrench.wrench.force.y = 0
wrench.wrench.torque.z = 0
self.wrench_pub.publish(wrench)
def update_callback(event):
global desired_state, desired_state_dot, state, stat_dot, state_dot_body, previous_error, enable
lock.acquire()
#print 'desired state', desired_state
#print 'current_state', state
def smallest_coterminal_angle(x):
return (x + math.pi) % (2 * math.pi) - math.pi
# sub pd-controller sans rise
e = numpy.concatenate([
desired_state[0:3] - state[0:3],
map(smallest_coterminal_angle, desired_state[3:6] - state[3:6])
]) # e_1 in paper
vbd = _jacobian_inv(state).dot(K.dot(e) + desired_state_dot)
e2 = vbd - state_dot_body
output = Ks.dot(e2)
#print 'output',output
lock.release()
if (not (odom_active)):
output = [0, 0, 0, 0, 0, 0]
if (enable):
controller_wrench.publish(
WrenchStamped(header=Header(
stamp=rospy.Time.now(),
frame_id="/base_link",
),
wrench=Wrench(
force=Vector3(x=output[0], y=output[1], z=0),
torque=Vector3(x=0, y=0, z=output[5]),
)))
def wrenchCallback(self, wrench_com_msg):
tau=[]
tau.append(wrench_com_msg.wrench.force.x)
tau.append(wrench_com_msg.wrench.force.y)
tau.append(wrench_com_msg.wrench.torque.z)
#update the required tau
self.tau_com=np.array(tau)
#organise into QP standard form
q,h,A,b = self.update()
#recover saved variables
n = self.n
f0 = self.f0
a0 = self.a0
l = self.l
try:
#solve for x = [df,da,s]
x = quadprog_solve_qp(self.P,q,self.G,h,A,b)
except ValueError:
#TODO publish error
rospy.logwarn("No good solution, reverting to previous good solution")
#set x to be all zeros (previous timestep thrusts conserved)
x = np.zeros(3*n)
#get change in thrusts
df = x[0:n]
#update thrusts
f = f0+df
#get change in angles
da = x[n:2*n]
#update angles
a = a0+da
#constrain angles to -pi<=a<pi
a[a>pi]=-pi-(a[a>pi]-pi)
a[a<=-pi]=pi+(pi+a[a<=-pi])
self.f0 = f
self.a0 = a
self.da = da
#get the solved wrench (really for debugging)
T = np.cos(a)
T = np.vstack((T,np.sin(a)))
temp = np.multiply(l[0,:],np.sin(a))-np.multiply(l[1,:],np.cos(a))
T = np.vstack((T,temp))
self.tau_sol = mul(T,f.T)
#publish the achieved wrench
tsol = WrenchStamped()
tsol.wrench.force.x = self.tau_sol[0]
tsol.wrench.force.y = self.tau_sol[1]
tsol.wrench.torque.z = self.tau_sol[2]
tsol.header.frame_id = 'base_link'
tsol.header.stamp = rospy.Time.now()
self.tsol_pub.publish(tsol)
#publish the new joint states
joint_states = JointState()
joint_states.header.stamp = rospy.Time.now()
joint_states.name = self.names
joint_states.position = self.a0
joint_states.velocity = self.da
joint_states.effort = self.f0
self.thrusters_pub.publish(joint_states)
def __on_packet(self, buf):
global last_joint_states, last_joint_states_lock
now = rospy.get_rostime()
stateRT = RobotStateRT.unpack(buf)
self.last_stateRT = stateRT
msg = JointState()
msg.header.stamp = now
msg.header.frame_id = "From real-time state data"
msg.name = joint_names
msg.position = [0.0] * 6
for i, q in enumerate(stateRT.q_actual):
msg.position[i] = q + joint_offsets.get(joint_names[i], 0.0)
msg.velocity = stateRT.qd_actual
msg.effort = [0] * 6
pub_joint_states.publish(msg)
with last_joint_states_lock:
last_joint_states = msg
wrench_msg = WrenchStamped()
wrench_msg.header.stamp = now
wrench_msg.wrench.force.x = stateRT.tcp_force[0]
wrench_msg.wrench.force.y = stateRT.tcp_force[1]
wrench_msg.wrench.force.z = stateRT.tcp_force[2]
wrench_msg.wrench.torque.x = stateRT.tcp_force[3]
wrench_msg.wrench.torque.y = stateRT.tcp_force[4]
wrench_msg.wrench.torque.z = stateRT.tcp_force[5]
pub_wrench.publish(wrench_msg)
def __init__(self):
self.vehicule = USV_char(config['characteristics'],
config['simulated_characteristics'],
config['actuators'],
environnement['environnement'])
# Definitions des messages
self.msgPose = Msg(PoseStamped)
self.msgTwist = Msg(TwistStamped)
self.msgAccel = Msg(AccelStamped)
self.constraints = {
'wind': WrenchStamped(),
'current': WrenchStamped()
}
def control_update(self, timer_event):
failsafe = False
if self.eta is None or self.v is None:
rospy.logwarn_throttle_identical(3, 'Odometry not yet received: Controller waiting...')
failsafe = True
if self.last_updated is None or rospy.Time.now().to_sec() - self.last_updated > self.timeout_duration:
failsafe = True
rospy.logwarn_throttle_identical(3, 'No controller command received recently: entering failsafe mode!')
if not failsafe:
eta = self.eta
v = self.v
ta = self.target_acc
ty = self.target_yaw_acc
roll_error = self.target_roll - eta[3]
pitch_error = self.target_pitch - eta[4]
# Limit error to [-pi, pi] to ensure that the robot rotates the least amount
# to hit the target roll/pitch:
if roll_error > np.pi:
roll_error -= 2*np.pi
if roll_error < -np.pi:
roll_error += 2 * np.pi
if pitch_error > np.pi:
pitch_error -= 2*np.pi
if pitch_error < -np.pi:
pitch_error += 2 * np.pi
# Compute efforts from PID:
roll_effort = self.roll_pid(-roll_error)
pitch_effort = self.pitch_pid(-pitch_error)
# Build the accel in world space:
vd_pid = [0, 0, 0, roll_effort, pitch_effort, 0]
# Convert worldspace accel to body frame:
R = Rotation.from_euler('xyz', eta[3:6]).inv()
target_acc_body = R.apply(ta)
target_yaw_body = R.apply([0, 0, ty])
vd_command = np.hstack((target_acc_body, target_yaw_body))
vd = np.array(vd_pid) + np.array(vd_command)
# Compute force required for target acceleration and current velocity:
# (Inverse dynamics)
tau = self.dyn.compute_tau(eta, v, vd)
else:
tau = [0] * 6
# Build and publish control wrench:
wrench = WrenchStamped()
wrench.header.stamp = rospy.Time.now()
wrench.header.frame_id = self.base_link
wrench.wrench.force = Vector3(tau[0], -tau[1], -tau[2])
wrench.wrench.torque = Vector3(tau[3], -tau[4], -tau[5])
self.pub_wrench.publish(wrench)
def __init__(self):
rospy.init_node('pd_controller')
self.current_joint_state = JointState()
self.current_ft_state = WrenchStamped() # record current state
# self.train_data_msg = TrainningData()
# self.lin_ds_out = DsOutput()
self.ctrl_freq = 200 #achieve the cmd in 0.1 sec
self.joint_names = [
"iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4",
"iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7"
]
#self.next_joint_state.joint_names = joint_names
# self.train_data_pub = rospy.Publisher('/train_data', numpy_msg(TrainningData),queue_size=10)
joint_state_sub = message_filters.Subscriber('/iiwa/joint_states',
numpy_msg(JointState))
ft_state_sub = message_filters.Subscriber('/ft_sensor/netft_data',
numpy_msg(WrenchStamped))
self.times_sync = message_filters.ApproximateTimeSynchronizer(
[joint_state_sub, ft_state_sub], 1000, slop=0.003)
#self.lin_ds_out_pub = rospy.Publisher('/lin_ds_out', numpy_msg(DsOutput),queue_size=10)
self.joint_cmd_pub = rospy.Publisher(
'/iiwa/PositionController/command',
numpy_msg(Float64MultiArray),
queue_size=10) #topic important
def publish_control_wrench(self, force):
if not self.odom_is_init:
return
# Apply saturation
for i in range(6):
if force[i] < -self._control_saturation:
force[i] = -self._control_saturation
elif force[i] > self._control_saturation:
force[i] = self._control_saturation
if not self.thrusters_only:
surge_speed = self._vehicle_model.vel[0]
self.publish_auv_command(surge_speed, force)
return
force_msg = WrenchStamped()
force_msg.header.stamp = rospy.Time.now()
force_msg.header.frame_id = '%s/%s' % (self._namespace, self._vehicle_model.body_frame_id)
force_msg.wrench.force.x = force[0]
force_msg.wrench.force.y = force[1]
force_msg.wrench.force.z = force[2]
force_msg.wrench.torque.x = force[3]
force_msg.wrench.torque.y = force[4]
force_msg.wrench.torque.z = force[5]
self._thrust_pub.publish(force_msg)
def find_bouyancy(
self, choice
): # Initial function used to upward force of buoyancy (used in determining drag in Z axis)
down_force = -.5 # Applies initial downward force in z axis
pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=30)
rospy.init_node('move_sub', anonymous=False)
force_msg = WrenchStamped()
force_msg.wrench.force.z = self.applied_force_down
pub.publish(force_msg) # publish wrench with force
rospy.loginfo('Moving sub down...')
rospy.sleep(self.time_of_apllied_force_down
) # node sleeps for some amount of time before continuing
force_msg.wrench.force.z = 0 # Applies 0 force which stops downward force
pub.publish(force_msg)
while not (rospy.is_shutdown()):
rospy.Subscriber("/odom", Odometry, self.get_velocity, choice)
rospy.sleep(1)
if self.velocity > 0.0:
rospy.loginfo('Appling a force down to calculate the bouyancy')
while not (rospy.is_shutdown()):
force_msg.wrench.force.z = down_force
pub.publish(force_msg)
down_force = down_force - .001
rospy.sleep(.01)
if self.velocity < 0.0:
break
rospy.loginfo('bouyancy found!')
break
self.bouyancy = abs(down_force)
rospy.loginfo('Bouyancy: {}'.format(self.bouyancy))
def __init__(self, frequency):
self.FEEDBACK_TIMEOUT = 2.0
self.wrench_input = WrenchStamped()
self.setpoint_valid = False
self.feedback_received = False
self.enabled = False
self.last_feedback = Range()
self.enable_server = rospy.Service('~enable',
auv_control_msgs.srv.EnableControl,
self.enable)
self.pid = Pid(0.0, 0.0, 0.0) # the right constants will
self.k_f = 0.0 # be set through dynamic reconfigure
self.server = dynamic_reconfigure.server.Server(
AltitudeControllerConfig, self.reconfigure)
self.pub = rospy.Publisher('~wrench_output', WrenchStamped)
rospy.Subscriber('~wrench_input', WrenchStamped,
self.wrenchInputCallback)
rospy.Subscriber('~altitude_request', Float32, self.setpointCallback)
period = rospy.rostime.Duration.from_sec(1.0 / frequency)
self.timer = rospy.Timer(period, self.updateOutput)
rospy.Subscriber('altitude', Range, self.altitudeCallback)
rospy.loginfo(
'Listening for altitude feedback to be published on '
'%s...', rospy.resolve_name('altitude'))
rospy.loginfo('Waiting for setpoint to be published on '
'%s...', rospy.resolve_name('~altitude_request'))
def pub_force_node():
# 运行话题
rospy.init_node('force_sensor_node')
pub = rospy.Publisher("/ft_sensor_topic", WrenchStamped, queue_size=100)
#建立线程接收六维力
t1 = threading.Thread(target=tcp_connect)
t1.start()
time.sleep(2)
#求取偏置
F_offset = np.zeros(6)
for i in range(10):
F_offset = F_offset + F
time.sleep(0.01)
F_offset = F_offset / 10.0
# 发送关节角度
rate = rospy.Rate(100)
k = 0
while not rospy.is_shutdown():
F1 = F - F_offset
command_force = WrenchStamped()
command_force.wrench.force.x = F1[0]
command_force.wrench.force.y = F1[1]
command_force.wrench.force.z = F1[2]
command_force.wrench.torque.x = F1[3]
command_force.wrench.torque.y = F1[4]
command_force.wrench.torque.z = F1[5]
pub.publish(command_force)
k = k + 1
#if(k/10==0):
print "六维力：", np.around(F1, 3)
rate.sleep()
def __init__(self):
self.VACUUM_GROUP = "kuka_vacuum_gripper"
self.CAMERA_GROUP = "kuka_camera"
self.IMPACT_GROUP = "kuka_impact_wrench"
self.VACUUM_EF_LINK = "kuka_vacuum_ef"
self.CAMERA_EF_LINK = "kuka_camera_ef"
self.IMPACT_EF_LINK = "kuka_impact_ef"
self.___PLANNING_JOINT_NAMES = [
"kuka_joint_a1", "kuka_joint_a2", "kuka_joint_a3", "kuka_joint_a4",
"kuka_joint_a5", "kuka_joint_a6", "kuka_joint_a7"
]
self.__FORCE_SENSOR_TOPIC = "/kuka_force_topic"
self.__VACUUM_STATUS_TOPIC = "/kuka_vacuum_topic"
# create move group commander
self.__vacuum_group_commander = MoveGroupCommander(self.VACUUM_GROUP)
self.__camera_group_commander = MoveGroupCommander(self.CAMERA_GROUP)
self.__impact_group_commander = MoveGroupCommander(self.IMPACT_GROUP)
# initialize vacuum services
self.__vacuum_on_service = rospy.ServiceProxy("/on", Empty)
self.__vacuum_off_service = rospy.ServiceProxy("/off", Empty)
# initialize feedback variables
self.__vacuum_status = Bool()
self.__force_sensor_value = WrenchStamped()
def set_link_wrench(self, force, torque, duration, starting_time):
ns = self.get_namespace().replace('/', '')
body_name = '%s/base_link' % ns
req = ApplyLinkWrench.Request()
self._body_force = np.array([float(self._body_force[i]) + force[i] for i in range(3)])
self._body_torque = np.array([float(self._body_torque[i]) + torque[i] for i in range(3)])
self._body_wrench_msg = WrenchStamped()
self._body_wrench_msg.header.stamp = self.get_clock().now().to_msg()
self._body_wrench_msg.header.frame_id = 'world'
self._body_wrench_msg.wrench.force = Vector3(x=self._body_force[0],
y=self._body_force[1],
z=self._body_force[2])
self._body_wrench_msg.wrench.torque = Vector3(x=self._body_torque[0],
y=self._body_torque[1],
z=self._body_torque[2])
(secs, nsecs) = float_sec_to_int_sec_nano(starting_time)
(d_secs, d_nsecs) = float_sec_to_int_sec_nano(duration)
req.link_name = body_name
req.reference_frame = 'world'
req.reference_point = Point(x=0., y=0., z=0.)
req.wrench = self._body_wrench_msg.wrench
req.start_time = rclpy.time.Time(seconds=secs, nanoseconds=nsecs).to_msg()
req.duration = rclpy.time.Duration(seconds=d_secs, nanoseconds=d_nsecs).to_msg()
future = self._service_cb['wrench'].call_async(req)
self.wait_for_service_completion(
future,
'Link wrench perturbation applied!, body_name=%s, t=%.2f s' % (body_name, time_in_float_sec(self.get_clock().now())),
'Failed to apply link wrench!, body_name=%s, t=%.2f s' % (body_name, time_in_float_sec(self.get_clock().now())))
def got_joy_msg(joy_msg):
# 6 start, 7 stop
if joy_msg.buttons[6]:
pass
elif joy_msg.buttons[7]:
pass
if joy_msg.buttons[1]:
pass
elif joy_msg.buttons[3]:
pass
axes = list(joy_msg.axes)
if np.fabs(axes[1]) < 0.1:
axes[1] = 0.0
if np.fabs(axes[0]) < 0.1:
axes[0] = 0.0
if np.fabs(axes[3]) < 0.1:
axes[3] = 0.0
for i in range(len(axes)):
axes[i] = np.round(axes[i], 2)
linear_x = axes[1] * max_x_force
linear_y = axes[0] * max_y_force
angular_z = axes[3] * -max_z_torque
seq = joy_msg.header.seq
wrench = WrenchStamped()
wrench.wrench = Wrench()
wrench.wrench.force = Vector3()
wrench.wrench.torque = Vector3()
wrench.wrench.force.x = linear_x
wrench.wrench.force.y = linear_y
wrench.wrench.force.z = 0
wrench.wrench.torque.x = 0
wrench.wrench.torque.y = 0
wrench.wrench.torque.z = angular_z
wrench.header.seq = seq
wrench.header.frame_id = '/robot'
wrench.header.stamp = joy_msg.header.stamp
pub.publish(wrench)
def force_cb(self, msg):
# start_crawling_time = time.time()
force_raw = np.array([
msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z,
msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z
],
dtype=np.float32)
force_tmp = force_raw - self.force_offset
if self.force_data_dimension == 6:
# Kalman filter predict
self.kf.predict()
# Kalman filter update
self.kf.update(force_tmp)
force_filtered = self.kf.x.reshape((self.force_data_dimension, ))
output_msg = WrenchStamped()
output_msg.header.frame_id = msg.header.frame_id
output_msg.wrench.force = Vector3(x=force_filtered[0],
y=force_filtered[1],
z=force_filtered[2])
output_msg.wrench.torque = Vector3(x=force_filtered[3],
y=force_filtered[4],
z=force_filtered[5])
output_msg.header.stamp = msg.header.stamp #rospy.Time.now()
self.pub_force.publish(output_msg)
elif self.force_data_dimension == 2:
force_tmp = np.array([force_tmp[1], force_tmp[5]],
dtype=np.float32)
# Kalman filter predict
self.kf.predict()
# Kalman filter update
self.kf.update(force_tmp)
force_filtered = self.kf.x.reshape((self.force_data_dimension, ))
output_msg = WrenchStamped()
output_msg.header.frame_id = msg.header.frame_id
output_msg.wrench.force = Vector3(x=0, y=force_filtered[0], z=0)
output_msg.wrench.torque = Vector3(x=0, y=0, z=force_filtered[1])
output_msg.header.stamp = msg.header.stamp #rospy.Time.now()
self.pub_force.publish(output_msg)
else:
rospy.logerr("Undefined filter data dimension")
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 publish_wrench(fx, fy, tau):
wrench = WrenchStamped()
wrench.wrench = Wrench()
wrench.wrench.force = Vector3()
wrench.wrench.torque = Vector3()
wrench.wrench.force.x = fx
wrench.wrench.force.y = fy
wrench.wrench.force.z = 0
wrench.wrench.torque.x = 0
wrench.wrench.torque.y = 0
wrench.wrench.torque.z = tau
wrench.header.seq = 0
wrench.header.frame_id = '/base_link'
wrench.header.stamp = rospy.Time.now()
wrench_pub.publish(wrench)
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
