def execute(self, userdata):
base_T_clone = tfu.tf_as_matrix(self.robot.tf_listener.lookupTransform(self.base_frame, self.frame_to_clone, rospy.Time(0)))
#print 'BASE FRAME', self.base_frame, 'FRAME TO CLONE', self.frame_to_clone
#print base_T_clone
pose = tfu.stamp_pose(tfu.mat_to_pose(base_T_clone), self.base_frame)
self.broadcast_transform_srv(self.new_frame_name, pose)
return 'succeeded'
def execute(self, userdata):
base_T_clone = tfu.tf_as_matrix(
self.robot.tf_listener.lookupTransform(self.base_frame,
self.frame_to_clone,
rospy.Time(0)))
pose = tfu.stamp_pose(tfu.mat_to_pose(base_T_clone), self.base_frame)
self.broadcast_transform_srv(self.new_frame_name, pose)
return 'succeeded'
def get_current_pose(self):
frame = str(self.frameline.text())
self.tf_listener.waitForTransform(frame, self.robot_frame_name, rospy.Time(), rospy.Duration(2.))
p_base = tfu.transform(frame, self.robot_frame_name, self.tf_listener) \
* tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
t, r = tfu.matrix_as_tf(p_base)
x = t[0]
y = t[1]
theta = tr.euler_from_quaternion(r)[2]
print x,y,theta
self.xline.setText(str(x))
self.yline.setText(str(y))
self.tline.setText(str(math.degrees(theta)))
def get_current_pose(self):
frame = str(self.frameline.text())
self.tf_listener.waitForTransform(frame, self.robot_frame_name,
rospy.Time(), rospy.Duration(2.))
p_base = tfu.transform(frame, self.robot_frame_name, self.tf_listener) \
* tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
t, r = tfu.matrix_as_tf(p_base)
x = t[0]
y = t[1]
theta = tr.euler_from_quaternion(r)[2]
print x, y, theta
self.xline.setText(str(x))
self.yline.setText(str(y))
self.tline.setText(str(math.degrees(theta)))
def get_current_pose(self):
frame_described_in = str(self.frameline.text())
left = ('Left' == str(self.arm_box.currentText()))
right = False
if not left:
right = True
arm_tip_frame = LinearMoveTool.RIGHT_TIP
else:
arm_tip_frame = LinearMoveTool.LEFT_TIP
self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Time(), rospy.Duration(2.))
p_arm = tfu.tf_as_matrix(self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0))) * tr.identity_matrix()
trans, rotation = tr.translation_from_matrix(p_arm), tr.quaternion_from_matrix(p_arm)
for value, vr in zip(trans, [self.xline, self.yline, self.zline]):
vr.setText(str(value))
for value, vr in zip(tr.euler_from_quaternion(rotation), [self.phi_line, self.theta_line, self.psi_line]):
vr.setText(str(np.degrees(value)))
def action_cb(self, msg):
rospy.loginfo('message that we got:\n' + str(msg))
self.controller_manager.cart_mode(self.arm)
trans_tolerance = msg.trans_tolerance #rospy.get_param("~translation_tolerance")
time_out = msg.time_out #rospy.get_param("~timeout")
self.rot_tolerance = math.radians(
rospy.get_param("~rotation_tolerance"))
#self._wait_for_pose_message()
success = False
r = rospy.Rate(100)
goal_ps = msg.goal
#relative_movement = msg.relative
trans_vel = msg.trans_vel
rot_vel = msg.rot_vel
if trans_vel <= 0:
trans_vel = .02
if rot_vel <= 0:
rot_vel = pi / 20.
tstart = rospy.get_time()
tmax = tstart + time_out
#self.controller_manager = ControllerManager()
rospy.loginfo('Goal is x %f y %f z %f in %s' %
(goal_ps.pose.position.x, goal_ps.pose.position.y,
goal_ps.pose.position.z, goal_ps.header.frame_id))
goal_torso = change_pose_stamped_frame(self.tf, goal_ps,
'torso_lift_link')
rospy.loginfo('BEFORE TIP Goal is x %f y %f z %f in %s' %
(goal_torso.pose.position.x, goal_torso.pose.position.y,
goal_torso.pose.position.z, goal_torso.header.frame_id))
rospy.loginfo('Tool Frame %s Controller Frame %s' %
(self.tool_frame, self.controller_frame))
tll_T_tip = pose_to_mat(goal_torso.pose)
tip_T_w = tf_as_matrix(
self.tf.lookupTransform(self.tool_frame, self.controller_frame,
rospy.Time(0)))
#print 'TRANSFORM', tip_T_w
tll_T_w = tll_T_tip * tip_T_w
goal_torso = stamp_pose(mat_to_pose(tll_T_w), 'torso_lift_link')
rospy.loginfo('AFTER TIP Goal is x %f y %f z %f in %s' %
(goal_torso.pose.position.x, goal_torso.pose.position.y,
goal_torso.pose.position.z, goal_torso.header.frame_id))
verbose = False
time_ang = None
min_ang_error = None
time_trans = None
min_trans_error = None
timed_out = False
while not rospy.is_shutdown():
cur_time = rospy.get_time()
gripper_matrix = tfu.tf_as_matrix(
self.tf.lookupTransform('torso_lift_link',
self.controller_frame, rospy.Time(0)))
gripper_ps = stamp_pose(mat_to_pose(gripper_matrix),
'torso_lift_link')
#Someone preempted us!
if self.linear_movement_as.is_preempt_requested():
#Stop our motion
self.target_pub.publish(
stamp_pose(gripper_ps.pose, gripper_ps.header.frame_id))
self.linear_movement_as.set_preempted()
rospy.loginfo('action_cb: PREEMPTED!')
break
#Calc feedback
if verbose:
print 'current_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % (
gripper_ps.pose.position.x, gripper_ps.pose.position.y,
gripper_ps.pose.position.z, gripper_ps.pose.orientation.x,
gripper_ps.pose.orientation.y,
gripper_ps.pose.orientation.z,
gripper_ps.pose.orientation.w, gripper_ps.header.frame_id)
print 'goal_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % (
goal_torso.pose.position.x, goal_torso.pose.position.y,
goal_torso.pose.position.z, goal_torso.pose.orientation.x,
goal_torso.pose.orientation.y,
goal_torso.pose.orientation.z,
goal_torso.pose.orientation.w, goal_torso.header.frame_id)
trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf)
feedback = ptp.LinearMovementFeedback(
gm.Vector3(trans[0, 0], trans[1, 0], trans[2, 0]))
self.linear_movement_as.publish_feedback(feedback)
#Reached goal
trans_mag = np.linalg.norm(trans)
if verbose:
print trans.T, 'trans_mag', trans_mag, 'ang', abs(
ang), 'rot toler', self.rot_tolerance
if min_trans_error == None or min_trans_error == None:
min_trans_error = trans_mag
min_ang_error = abs(ang)
time_ang = cur_time
time_trans = cur_time
if trans_mag < min_trans_error:
min_trans_error = trans_mag
time_trans = cur_time
if abs(ang) < min_ang_error:
min_ang_error = abs(ang)
time_ang = cur_time
if trans_tolerance > trans_mag and self.rot_tolerance > abs(ang):
rospy.loginfo('action_cb: reached goal.')
break
#Timed out! is this a failure?
if cur_time > tmax:
rospy.loginfo('action_cb: timed out.')
timed_out = True
break
#if it has been a while since we made progress
if trans_mag > min_trans_error and (cur_time -
time_trans) > self.stall_time:
rospy.loginfo('action_cb: stalled.')
break
# tends to not stall on angle so don't look out for this case
#if abs(ang) > min_ang_error and (cur_time - time_ang) > self.stall_time:
# rospy.loginfo('action_cb: stalled.')
# break
#Send controls
clamped_target = self.clamp_pose(goal_torso,
trans_vel,
rot_vel,
ref_pose=gripper_ps)
if verbose:
print 'clamped_target', clamped_target.pose.position.x, clamped_target.pose.position.y,
print clamped_target.pose.position.z, clamped_target.header.frame_id, '\n'
#break
self.target_pub.publish(clamped_target)
#break
trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf)
result = ptp.LinearMovementResult(
gm.Vector3(trans[0, 0], trans[1, 0], trans[2, 0]), 'unknown')
if trans_tolerance > np.linalg.norm(trans):
rospy.loginfo('SUCCEEDED! %.3f ang %.3f' %
(np.linalg.norm(trans), np.degrees(ang)))
result.message = 'succeeded'
self.linear_movement_as.set_succeeded(result)
else:
rospy.loginfo('ABORTED! %.3f ang %.3f' %
(np.linalg.norm(trans), np.degrees(ang)))
if timed_out:
result.message = 'timed_out'
else:
result.message = 'goal_not_reached'
self.linear_movement_as.set_aborted(result)
def get_pose(self):
p_base = tfu.transform('map', 'base_footprint', self.tflistener) \
* tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
return tfu.matrix_as_tf(p_base)
def action_cb(self, msg):
rospy.loginfo('message that we got:\n' + str(msg))
self.controller_manager.cart_mode(self.arm)
trans_tolerance = msg.trans_tolerance #rospy.get_param("~translation_tolerance")
time_out = msg.time_out #rospy.get_param("~timeout")
self.rot_tolerance = math.radians(rospy.get_param("~rotation_tolerance"))
#self._wait_for_pose_message()
success = False
r = rospy.Rate(100)
goal_ps = msg.goal
#relative_movement = msg.relative
trans_vel = msg.trans_vel
rot_vel = msg.rot_vel
if trans_vel <= 0:
trans_vel = .02
if rot_vel <= 0:
rot_vel = pi/20.
tstart = rospy.get_time()
tmax = tstart + time_out
#self.controller_manager = ControllerManager()
rospy.loginfo('Goal is x %f y %f z %f in %s' % (goal_ps.pose.position.x, goal_ps.pose.position.y,
goal_ps.pose.position.z, goal_ps.header.frame_id))
goal_torso = change_pose_stamped_frame(self.tf, goal_ps, 'torso_lift_link')
rospy.loginfo('BEFORE TIP Goal is x %f y %f z %f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y,
goal_torso.pose.position.z, goal_torso.header.frame_id))
rospy.loginfo('Tool Frame %s Controller Frame %s' % (self.tool_frame, self.controller_frame))
tll_T_tip = pose_to_mat(goal_torso.pose)
tip_T_w = tf_as_matrix(self.tf.lookupTransform(self.tool_frame, self.controller_frame, rospy.Time(0)))
#print 'TRANSFORM', tip_T_w
tll_T_w = tll_T_tip * tip_T_w
goal_torso = stamp_pose(mat_to_pose(tll_T_w), 'torso_lift_link')
rospy.loginfo('AFTER TIP Goal is x %f y %f z %f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y,
goal_torso.pose.position.z, goal_torso.header.frame_id))
verbose = False
time_ang = None
min_ang_error = None
time_trans = None
min_trans_error = None
timed_out = False
while not rospy.is_shutdown():
cur_time = rospy.get_time()
gripper_matrix = tfu.tf_as_matrix(self.tf.lookupTransform('torso_lift_link', self.controller_frame, rospy.Time(0)))
gripper_ps = stamp_pose(mat_to_pose(gripper_matrix), 'torso_lift_link')
#Someone preempted us!
if self.linear_movement_as.is_preempt_requested():
#Stop our motion
self.target_pub.publish(stamp_pose(gripper_ps.pose, gripper_ps.header.frame_id))
self.linear_movement_as.set_preempted()
rospy.loginfo('action_cb: PREEMPTED!')
break
#Calc feedback
if verbose:
print 'current_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % (gripper_ps.pose.position.x, gripper_ps.pose.position.y, gripper_ps.pose.position.z,
gripper_ps.pose.orientation.x, gripper_ps.pose.orientation.y, gripper_ps.pose.orientation.z,
gripper_ps.pose.orientation.w, gripper_ps.header.frame_id)
print 'goal_pose %.3f %.3f %.3f, rot %.3f %.3f %.3f %.3f in %s' % (goal_torso.pose.position.x, goal_torso.pose.position.y, goal_torso.pose.position.z,
goal_torso.pose.orientation.x, goal_torso.pose.orientation.y, goal_torso.pose.orientation.z,
goal_torso.pose.orientation.w, goal_torso.header.frame_id)
trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf)
feedback = ptp.LinearMovementFeedback(gm.Vector3(trans[0,0], trans[1,0], trans[2,0]))
self.linear_movement_as.publish_feedback(feedback)
#Reached goal
trans_mag = np.linalg.norm(trans)
if verbose:
print trans.T, 'trans_mag', trans_mag, 'ang', abs(ang), 'rot toler', self.rot_tolerance
if min_trans_error == None or min_trans_error == None:
min_trans_error = trans_mag
min_ang_error = abs(ang)
time_ang = cur_time
time_trans = cur_time
if trans_mag < min_trans_error:
min_trans_error = trans_mag
time_trans = cur_time
if abs(ang) < min_ang_error:
min_ang_error = abs(ang)
time_ang = cur_time
if trans_tolerance > trans_mag and self.rot_tolerance > abs(ang):
rospy.loginfo('action_cb: reached goal.')
break
#Timed out! is this a failure?
if cur_time > tmax:
rospy.loginfo('action_cb: timed out.')
timed_out = True
break
#if it has been a while since we made progress
if trans_mag > min_trans_error and (cur_time - time_trans) > self.stall_time:
rospy.loginfo('action_cb: stalled.')
break
# tends to not stall on angle so don't look out for this case
#if abs(ang) > min_ang_error and (cur_time - time_ang) > self.stall_time:
# rospy.loginfo('action_cb: stalled.')
# break
#Send controls
clamped_target = self.clamp_pose(goal_torso, trans_vel, rot_vel, ref_pose=gripper_ps)
if verbose:
print 'clamped_target', clamped_target.pose.position.x, clamped_target.pose.position.y,
print clamped_target.pose.position.z, clamped_target.header.frame_id, '\n'
#break
self.target_pub.publish(clamped_target)
#break
trans, ang, _ = pose_distance(gripper_ps, goal_torso, self.tf)
result = ptp.LinearMovementResult(gm.Vector3(trans[0,0], trans[1,0], trans[2,0]), 'unknown')
if trans_tolerance > np.linalg.norm(trans):
rospy.loginfo( 'SUCCEEDED! %.3f ang %.3f' % (np.linalg.norm(trans), np.degrees(ang)))
result.message = 'succeeded'
self.linear_movement_as.set_succeeded(result)
else:
rospy.loginfo('ABORTED! %.3f ang %.3f' % (np.linalg.norm(trans), np.degrees(ang)))
if timed_out:
result.message = 'timed_out'
else:
result.message = 'goal_not_reached'
self.linear_movement_as.set_aborted(result)
def get_pose(self):
p_base = tfu.transform('map', 'base_footprint', self.tflistener) \
* tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0)))
return tfu.matrix_as_tf(p_base)
