def wrench_callback(self, state):
#calculating force estimate from position error (does not compensate for motion error due to dynamics)
q = self.robot.get_joint_angles()
pos,_ = self.robot.kinematics.FK(q)
x_err = np.matrix([state.point.x-pos[0,0], state.point.y-pos[1,0], state.point.z-pos[2,0]]).T
##########This was for debugging purposes
# ps = PointStamped()
# ps.header.frame_id = '/torso_lift_link'
# ps.header.stamp = rospy.get_rostime()
# ps.point.x = x_dot_err[0,0]
# ps.point.y = x_dot_err[0,0]
# ps.point.z = 0
#self.err_pub.publish(ps)
feedback = -1.0*self.kp*x_err # - self.kd*x_dot_err This term is now included in real-time omni driver
#find and use the rotation matrix from wrench to torso
df_R_ee = tfu.rotate(self.dest_frame, 'torso_lift_link', self.tflistener)
wr_df = self.force_scaling*np.array(tr.translation_from_matrix(df_R_ee * tfu.translation_matrix([feedback[0,0], feedback[1,0], feedback[2,0]])))
#limiting the max and min force feedback sent to omni
wr_df = np.where(wr_df>self.omni_max_limit, self.omni_max_limit, wr_df)
wr_df = np.where(wr_df<self.omni_min_limit, self.omni_min_limit, wr_df)
wr = OmniFeedback()
wr.force.x = wr_df[0]
wr.force.y = wr_df[1]
wr.force.z = wr_df[2]
if self.enable == True:
self.omni_fb.publish(wr)
def transform_point(self, point_stamped):
point_head = point_stamped.point
#Tranform into base link
base_T_head = tfu.transform(self.target_frame, point_stamped.header.frame_id, self.tf_listener)
point_mat_head = tfu.translation_matrix([point_head.x, point_head.y, point_head.z])
point_mat_base = base_T_head * point_mat_head
t_base, _ = tfu.matrix_as_tf(point_mat_base)
return np.matrix(t_base).T
def __init__(
self,
robot,
center_in_torso_frame, #=[1,.3,-1],
scaling_in_base_frame, #=[3.5, 3., 5.],
omni_name, #='omni1',
set_goal_pose_topic, # = 'l_cart/command_pose',
tfbroadcast=None, #=None,
tflistener=None): #=None):
#threading.Thread.__init__(self)
self.enabled = False
self.zero_out_forces = True
self.robot = robot
#self.X_BOUNDS = [.3, 1.5] #Bound translation of PR2 arms in the X direction (in torso link frame)
self.kPos = 15.
self.kVel = 0.5
self.kPos_close = 70.
self.omni_name = omni_name
self.center_in_torso_frame = center_in_torso_frame
self.scaling_in_base_frame = scaling_in_base_frame
self.tflistener = tflistener
self.tfbroadcast = tfbroadcast
self.prev_dt = 0.0
q = self.robot.get_joint_angles()
self.tip_tt, self.tip_tq = self.robot.kinematics.FK(
q, self.robot.kinematics.n_jts)
# self.tip_tt = np.zeros((3,1))
# self.tip_tq = np.zeros((4,1))
self.teleop_marker_pub = rospy.Publisher('/teleop/viz/goal', Marker)
rate = rospy.Rate(100.0)
self.omni_fb = rospy.Publisher(self.omni_name + '_force_feedback',
OmniFeedback)
self.set_goal_pub = rospy.Publisher(set_goal_pose_topic, PoseStamped)
rospy.loginfo(
'Attempting to calculate scaling from omni to arm workspace')
success = False
while not success and (not rospy.is_shutdown()):
rate.sleep()
try:
m = tfu.translation_matrix(self.scaling_in_base_frame)
vec_l0 = tr.translation_from_matrix(
tfu.rotate(self.omni_name + '_center', '/torso_lift_link',
self.tflistener) * m)
self.scale_omni_l0 = np.abs(vec_l0)
success = True
except tf.LookupException, e:
pass
except tf.ConnectivityException, e:
pass
def transform_point(self, point_stamped):
point_head = point_stamped.point
#Tranform into base link
target_link = '/base_link'
base_T_head = tfu.transform(target_link, point_stamped.header.frame_id, self.tflistener)
point_mat_head = tfu.translation_matrix([point_head.x, point_head.y, point_head.z])
point_mat_base = base_T_head * point_mat_head
t_base, o_base = tfu.matrix_as_tf(point_mat_base)
#Calculate angle robot should face
angle = math.atan2(t_base[1], t_base[0])
q = tf.transformations.quaternion_from_euler(0, 0, angle)
return (t_base, q, target_link)
def transform_point(self, point_stamped):
point_head = point_stamped.point
#Tranform into base link
target_link = '/base_link'
base_T_head = tfu.transform(target_link, point_stamped.header.frame_id, self.tflistener)
point_mat_head = tfu.translation_matrix([point_head.x, point_head.y, point_head.z])
point_mat_base = base_T_head * point_mat_head
t_base, o_base = tfu.matrix_as_tf(point_mat_base)
#Calculate angle robot should face
angle = math.atan2(t_base[1], t_base[0])
q_base = tf.transformations.quaternion_from_euler(0, 0, angle)
return (t_base, q_base, target_link)
def __init__(self, robot,
center_in_torso_frame, #=[1,.3,-1],
scaling_in_base_frame, #=[3.5, 3., 5.],
omni_name, #='omni1',
set_goal_pose_topic, # = 'l_cart/command_pose',
tfbroadcast=None, #=None,
tflistener=None): #=None):
#threading.Thread.__init__(self)
self.enabled = False
self.zero_out_forces = True
self.robot = robot
#self.X_BOUNDS = [.3, 1.5] #Bound translation of PR2 arms in the X direction (in torso link frame)
self.kPos = 15.
self.kVel = 0.5
self.kPos_close = 70.
self.omni_name = omni_name
self.center_in_torso_frame = center_in_torso_frame
self.scaling_in_base_frame = scaling_in_base_frame
self.tflistener = tflistener
self.tfbroadcast = tfbroadcast
self.prev_dt = 0.0
q = self.robot.get_joint_angles()
self.tip_tt, self.tip_tq = self.robot.kinematics.FK(q, self.robot.kinematics.n_jts)
# self.tip_tt = np.zeros((3,1))
# self.tip_tq = np.zeros((4,1))
self.teleop_marker_pub = rospy.Publisher('/teleop/viz/goal', Marker)
rate = rospy.Rate(100.0)
self.omni_fb = rospy.Publisher(self.omni_name + '_force_feedback', OmniFeedback)
self.set_goal_pub = rospy.Publisher(set_goal_pose_topic, PoseStamped)
rospy.loginfo('Attempting to calculate scaling from omni to arm workspace')
success = False
while not success and (not rospy.is_shutdown()):
rate.sleep()
try:
m = tfu.translation_matrix(self.scaling_in_base_frame)
vec_l0 = tr.translation_from_matrix(tfu.rotate(self.omni_name + '_center', '/torso_lift_link', self.tflistener)*m)
self.scale_omni_l0 = np.abs(vec_l0)
success = True
except tf.LookupException, e:
pass
except tf.ConnectivityException, e:
pass
def wrench_callback(self, state):
# calculating force estimate from position error (does not compensate for motion error due to dynamics)
x_err = np.matrix([state.x_err.linear.x, state.x_err.linear.y, state.x_err.linear.z]).T
x_dot = np.matrix([state.xd.linear.x, state.xd.linear.y, state.xd.linear.z]).T
feedback = -1.0 * self.kp * x_err - self.kd * x_dot
# currently the calculated force feedback is published but not to omni, we use the velocity limited state from the controller
# wr_ee = [state.F.force.x, state.F.force.y, state.F.force.z]
wr_ee = [feedback[0, 0], feedback[1, 0], feedback[2, 0]]
# this is a simple FIR filter designed in Matlab to smooth the force estimate
shift_right = np.array(self.history[:, 0 : self.FIR.size - 1])
new_col = np.array(wr_ee).reshape((3, 1))
self.history = np.matrix(np.hstack((new_col, shift_right)))
wr_ee_filt = self.history * self.FIR
# find and use the rotation matrix from wrench to torso
df_R_ee = tfu.rotate(self.dest_frame, "torso_lift_link", self.tflistener) * tfu.rotate(
"torso_lift_link", self.wrench_frame, self.tflistener
)
wr_df = self.force_scaling * np.array(
tr.translation_from_matrix(
df_R_ee * tfu.translation_matrix([wr_ee_filt[0, 0], wr_ee_filt[1, 0], wr_ee_filt[2, 0]])
)
)
# limiting the max and min force feedback sent to omni
wr_df = np.where(wr_df > self.omni_max_limit, self.omni_max_limit, wr_df)
wr_df = np.where(wr_df < self.omni_min_limit, self.omni_min_limit, wr_df)
wr = Wrench()
wr.force.x = wr_df[0, 0]
wr.force.y = wr_df[1, 0]
wr.force.z = wr_df[2, 0]
# publishing of two different wrenches calculated, DELETE first when all finished
if self.enable == False:
self.filtered_fb.publish(wr)
if self.enable == True:
self.omni_fb.publish(wr)
def wrench_callback(self, state):
#calculating force estimate from position error (does not compensate for motion error due to dynamics)
q = self.robot.get_joint_angles()
pos, _ = self.robot.kinematics.FK(q)
x_err = np.matrix([
state.point.x - pos[0, 0], state.point.y - pos[1, 0],
state.point.z - pos[2, 0]
]).T
##########This was for debugging purposes
# ps = PointStamped()
# ps.header.frame_id = '/torso_lift_link'
# ps.header.stamp = rospy.get_rostime()
# ps.point.x = x_dot_err[0,0]
# ps.point.y = x_dot_err[0,0]
# ps.point.z = 0
#self.err_pub.publish(ps)
feedback = -1.0 * self.kp * x_err # - self.kd*x_dot_err This term is now included in real-time omni driver
#find and use the rotation matrix from wrench to torso
df_R_ee = tfu.rotate(self.dest_frame, 'torso_lift_link',
self.tflistener)
wr_df = self.force_scaling * np.array(
tr.translation_from_matrix(df_R_ee * tfu.translation_matrix(
[feedback[0, 0], feedback[1, 0], feedback[2, 0]])))
#limiting the max and min force feedback sent to omni
wr_df = np.where(wr_df > self.omni_max_limit, self.omni_max_limit,
wr_df)
wr_df = np.where(wr_df < self.omni_min_limit, self.omni_min_limit,
wr_df)
wr = OmniFeedback()
wr.force.x = wr_df[0]
wr.force.y = wr_df[1]
wr.force.z = wr_df[2]
if self.enable == True:
self.omni_fb.publish(wr)
def follow_point_cb(self, point_stamped):
point_head = point_stamped.point
base_T_head = tfu.transform('/base_link', point_stamped.header.frame_id, self.tflistener)
point_mat_head = tfu.translation_matrix([point.x, point.y, point.z])
point_mat_base = base_T_head * point_mat_head
t_base, o_base = tfu.matrix_as_tf(point_mat_base)
x = t_base[0]
y = t_base[1]
angle = math.atan2(y, x)
ps = PoseStamped()
ps.header.frame_id = '/base_link'
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = 0.
q = tf.transformations.quaternion_from_euler(angle, 0, 0)
ps.pose.orientation.x = q[0]
ps.pose.orientation.y = q[1]
ps.pose.orientation.z = q[2]
ps.pose.orientation.w = q[3]
self.move_pub.publish(ps)
def wrench_callback(self, state):
#calculating force estimate from position error (does not compensate for motion error due to dynamics)
x_err = np.matrix([state.x_err.linear.x, state.x_err.linear.y, state.x_err.linear.z]).T
x_dot = np.matrix([state.xd.linear.x, state.xd.linear.y, state.xd.linear.z]).T
feedback = -1.0*self.kp*x_err-self.kd*x_dot
#currently the calculated force feedback is published but not to omni, we use the velocity limited state from the controller
# wr_ee = [state.F.force.x, state.F.force.y, state.F.force.z]
wr_ee = [feedback[0,0], feedback[1,0], feedback[2,0]]
#this is a simple FIR filter designed in Matlab to smooth the force estimate
shift_right = np.array(self.history[:,0:self.FIR.size-1])
new_col = np.array(wr_ee).reshape((3,1))
self.history = np.matrix(np.hstack((new_col, shift_right)))
wr_ee_filt = self.history*self.FIR
#find and use the rotation matrix from wrench to torso
df_R_ee = tfu.rotate(self.dest_frame, 'torso_lift_link', self.tflistener) * \
tfu.rotate('torso_lift_link', self.wrench_frame, self.tflistener)
wr_df = self.force_scaling*np.array(tr.translation_from_matrix(df_R_ee * tfu.translation_matrix([wr_ee_filt[0,0], wr_ee_filt[1,0], wr_ee_filt[2,0]])))
#limiting the max and min force feedback sent to omni
wr_df = np.where(wr_df>self.omni_max_limit, self.omni_max_limit, wr_df)
wr_df = np.where(wr_df<self.omni_min_limit, self.omni_min_limit, wr_df)
wr = Wrench()
wr.force.x = wr_df[0,0]
wr.force.y = wr_df[1,0]
wr.force.z = wr_df[2,0]
#publishing of two different wrenches calculated, DELETE first when all finished
if self.enable == False:
self.filtered_fb.publish(wr)
if self.enable == True:
self.omni_fb.publish(wr)
