def create_goal_pose(self, x, y, z, gripper_pose):
point = [x, y, z]
point = self.transform_in_frame(point, gripper_pose, -self.GRIPPER_POINT).tolist()
pose = point + gripper_pose.tolist()
goal_pose = cf.create_pose_stamped(pose, "torso_lift_link")
goal_pose.header.stamp = rospy.Time.now()
return goal_pose
def _move_cartesian_ik(self, position, orientation, \
stop_funcs=[], timeout = 30., settling_time = 0.25, \
frame='base_link', vel=.15):
#pdb.set_trace()
#self.arm_obj.set_cart_pose_ik(cart_pose, total_time=motion_length, frame=frame, block=False)
#cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist()))
self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10))
toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener)
cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist()))
cart_pose = cart_pose * toolframe_T_ikframe
position, orientation = tfu.matrix_as_tf(cart_pose)
#goal_pose_ps = create_pose_stamped(position.A1.tolist() + orientation.A1.tolist(), frame)
goal_pose_ps = cf.create_pose_stamped(position.tolist() + orientation.tolist(), frame)
r = self.reactive_gr.cm.move_cartesian_ik(goal_pose_ps, blocking=0, step_size=.005, \
pos_thres=.02, rot_thres=.1, timeout=rospy.Duration(timeout),
settling_time=rospy.Duration(settling_time), vel=vel)
if not (r == 'sent goal' or r == 'success'):
return r
#move_cartesian_ik(self, goal_pose, collision_aware = 0, blocking = 1,
# step_size = .005, pos_thres = .02, rot_thres = .1,
# timeout = rospy.Duration(30.),
# settling_time = rospy.Duration(0.25), vel = .15):
stop_trigger = None
done_time = None
start_time = rospy.get_rostime()
while stop_trigger == None:
for f, name in stop_funcs:
if f():
self.arm_obj.stop_trajectory_execution()
stop_trigger = name
rospy.loginfo('"%s" requested that motion should be stopped.' % (name))
break
if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout):
rospy.loginfo("_move_cartesian_ik: motion timed out")
break
if (not done_time) and (not self.arm_obj.has_active_goal()):
#rospy.loginfo("check_cartesian_done returned 1")
done_time = rospy.get_rostime()
if done_time and rospy.get_rostime() - done_time > rospy.Duration(settling_time):
rospy.loginfo("_move_cartesian_ik: done settling")
break
if stop_trigger == 'pressure_safety':
print 'ROBOT SAFETY ERROR'
#raise RobotSafetyError(stop_trigger)
return stop_trigger
def _move_cartesian_ik(self, position, orientation, \
stop_funcs=[], timeout = 30., settling_time = 0.25, \
frame='base_link', vel=.15):
#pdb.set_trace()
#self.arm_obj.set_cart_pose_ik(cart_pose, total_time=motion_length, frame=frame, block=False)
#cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist()))
self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame,
rospy.Time(), rospy.Duration(10))
toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame,
self.tf_listener)
cart_pose = tfu.tf_as_matrix(
(position.A1.tolist(), orientation.A1.tolist()))
cart_pose = cart_pose * toolframe_T_ikframe
position, orientation = tfu.matrix_as_tf(cart_pose)
#goal_pose_ps = create_pose_stamped(position.A1.tolist() + orientation.A1.tolist(), frame)
goal_pose_ps = cf.create_pose_stamped(
position.tolist() + orientation.tolist(), frame)
r = self.reactive_gr.cm.move_cartesian_ik(goal_pose_ps, blocking=0, step_size=.005, \
pos_thres=.02, rot_thres=.1, timeout=rospy.Duration(timeout),
settling_time=rospy.Duration(settling_time), vel=vel)
if not (r == 'sent goal' or r == 'success'):
return r
#move_cartesian_ik(self, goal_pose, collision_aware = 0, blocking = 1,
# step_size = .005, pos_thres = .02, rot_thres = .1,
# timeout = rospy.Duration(30.),
# settling_time = rospy.Duration(0.25), vel = .15):
stop_trigger = None
done_time = None
start_time = rospy.get_rostime()
while stop_trigger == None:
for f, name in stop_funcs:
if f():
self.arm_obj.stop_trajectory_execution()
stop_trigger = name
rospy.loginfo(
'"%s" requested that motion should be stopped.' %
(name))
break
if timeout != 0. and rospy.get_rostime(
) - start_time > rospy.Duration(timeout):
rospy.loginfo("_move_cartesian_ik: motion timed out")
break
if (not done_time) and (not self.arm_obj.has_active_goal()):
#rospy.loginfo("check_cartesian_done returned 1")
done_time = rospy.get_rostime()
if done_time and rospy.get_rostime() - done_time > rospy.Duration(
settling_time):
rospy.loginfo("_move_cartesian_ik: done settling")
break
if stop_trigger == 'pressure_safety':
print 'ROBOT SAFETY ERROR'
#raise RobotSafetyError(stop_trigger)
return stop_trigger
def _move_cartesian_cart(self, position, orientation, \
stop_funcs=[], timeout = 3.0, settling_time = 0.5, frame='base_link'):
# TODO: Test this function...
# Transform the pose from 'frame' to 'base_link'
self.tf_listener.waitForTransform('base_link', frame, rospy.Time(),
rospy.Duration(10))
frame_T_base = tfu.transform('base_link', frame, self.tf_listener)
init_cart_pose = tfu.tf_as_matrix(
(position.A1.tolist(), orientation.A1.tolist()))
base_cart_pose = frame_T_base * init_cart_pose
# Get IK from tool frame to wrist frame for control input
self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame,
rospy.Time(), rospy.Duration(10))
toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame,
self.tf_listener)
#base_cart_pose = tfu.tf_as_matrix((base_position.A1.tolist(), base_orientation.A1.tolist()))
base_cart_pose = base_cart_pose * toolframe_T_ikframe
base_position, base_orientation = tfu.matrix_as_tf(base_cart_pose)
pose_stamped = cf.create_pose_stamped(base_position.tolist() +
base_orientation.tolist())
rg = self.reactive_gr
rg.check_preempt()
#send the goal to the Cartesian controllers
#rospy.loginfo("sending goal to Cartesian controllers")
(pos, rot) = cf.pose_stamped_to_lists(rg.cm.tf_listener, pose_stamped,
'base_link')
rg.move_cartesian_step(pos + rot, timeout, settling_time)
#watch the fingertip/palm sensors until the controllers are done and then some
start_time = rospy.get_rostime()
done_time = None
#stopped = False
stop_trigger = None
#print 'enterning loop'
stop_detector = ArmStoppedDetector()
while (1):
rg.check_preempt()
if len(stop_funcs) > 0:
for f, name in stop_funcs:
if f():
rg.cm.switch_to_joint_mode()
rg.cm.freeze_arm()
#stopped = True
stop_trigger = name
rospy.loginfo(
'"%s" requested that motion should be stopped.' %
(name))
break
if stop_trigger != None:
break
#if stop_func != None and stop_func():
# rg.cm.switch_to_joint_mode()
# rg.cm.freeze_arm()
# stopped = True
# break
#check if we're actually there
if rg.cm.check_cartesian_near_pose(pose_stamped, .0025, .05):
rospy.loginfo("_move_cartesian_cart: reached pose")
#stop_trigger = 'at_pose'
break
stop_detector.record_diff(self.arm_obj.pose_cartesian())
if stop_detector.is_stopped():
rospy.loginfo("_move_cartesian_cart: arm stopped")
#stop_trigger = 'stopped'
break
# if rg.cm.check_cartesian_really_done(pose_stamped, .0025, .05):
# #rospy.loginfo("actually got there")
# break
#
# #check if the controllers think we're done
# if not done_time and rg.cm.check_cartesian_done():
# #rospy.loginfo("check_cartesian_done returned 1")
# done_time = rospy.get_rostime()
# #done settling
# if done_time and rospy.get_rostime() - done_time > rospy.Duration(settling_time):
# rospy.loginfo("done settling")
# break
#timed out
#if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout):
# rospy.loginfo("_move_cartesian_cart: timed out")
# break
#if stop_trigger == 'pressure_safety' or stop_trigger == 'self_collision':
if stop_trigger == 'pressure_safety':
print 'ROBOT SAFETY ERROR'
#raise RobotSafetyError(stop_trigger)
#name = ut.formatted_time() + '_stop_detector.pkl'
#print 'saved', name
#ut.save_pickle(stop_detector, name)
return stop_trigger
def _move_cartesian_cart(self, position, orientation, \
stop_funcs=[], timeout = 3.0, settling_time = 0.5, frame='base_link'):
# TODO: Test this function...
# Transform the pose from 'frame' to 'base_link'
self.tf_listener.waitForTransform('base_link', frame, rospy.Time(),
rospy.Duration(10))
frame_T_base = tfu.transform('base_link', frame, self.tf_listener)
init_cart_pose = tfu.tf_as_matrix((position.A1.tolist(),
orientation.A1.tolist()))
base_cart_pose = frame_T_base*init_cart_pose
# Get IK from tool frame to wrist frame for control input
self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10))
toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener)
#base_cart_pose = tfu.tf_as_matrix((base_position.A1.tolist(), base_orientation.A1.tolist()))
base_cart_pose = base_cart_pose * toolframe_T_ikframe
base_position, base_orientation = tfu.matrix_as_tf(base_cart_pose)
pose_stamped = cf.create_pose_stamped(base_position.tolist() + base_orientation.tolist())
rg = self.reactive_gr
rg.check_preempt()
#send the goal to the Cartesian controllers
#rospy.loginfo("sending goal to Cartesian controllers")
(pos, rot) = cf.pose_stamped_to_lists(rg.cm.tf_listener, pose_stamped, 'base_link')
rg.move_cartesian_step(pos+rot, timeout, settling_time)
#watch the fingertip/palm sensors until the controllers are done and then some
start_time = rospy.get_rostime()
done_time = None
#stopped = False
stop_trigger = None
#print 'enterning loop'
stop_detector = ArmStoppedDetector()
while(1):
rg.check_preempt()
if len(stop_funcs) > 0:
for f, name in stop_funcs:
if f():
rg.cm.switch_to_joint_mode()
rg.cm.freeze_arm()
#stopped = True
stop_trigger = name
rospy.loginfo('"%s" requested that motion should be stopped.' % (name))
break
if stop_trigger != None:
break
#if stop_func != None and stop_func():
# rg.cm.switch_to_joint_mode()
# rg.cm.freeze_arm()
# stopped = True
# break
#check if we're actually there
if rg.cm.check_cartesian_near_pose(pose_stamped, .0025, .05):
rospy.loginfo("_move_cartesian_cart: reached pose")
#stop_trigger = 'at_pose'
break
stop_detector.record_diff(self.arm_obj.pose_cartesian())
if stop_detector.is_stopped():
rospy.loginfo("_move_cartesian_cart: arm stopped")
#stop_trigger = 'stopped'
break
# if rg.cm.check_cartesian_really_done(pose_stamped, .0025, .05):
# #rospy.loginfo("actually got there")
# break
#
# #check if the controllers think we're done
# if not done_time and rg.cm.check_cartesian_done():
# #rospy.loginfo("check_cartesian_done returned 1")
# done_time = rospy.get_rostime()
# #done settling
# if done_time and rospy.get_rostime() - done_time > rospy.Duration(settling_time):
# rospy.loginfo("done settling")
# break
#timed out
#if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout):
# rospy.loginfo("_move_cartesian_cart: timed out")
# break
#if stop_trigger == 'pressure_safety' or stop_trigger == 'self_collision':
if stop_trigger == 'pressure_safety':
print 'ROBOT SAFETY ERROR'
#raise RobotSafetyError(stop_trigger)
#name = ut.formatted_time() + '_stop_detector.pkl'
#print 'saved', name
#ut.save_pickle(stop_detector, name)
return stop_trigger
