def __init__(self):
rospy.init_node('tabletop_push_node', log_level=rospy.DEBUG)
self.torso_z_offset = rospy.get_param('~torso_z_offset', 0.15)
self.look_pt_x = rospy.get_param('~look_point_x', 0.45)
self.head_pose_cam_frame = rospy.get_param('~head_pose_cam_frame',
'openni_rgb_frame')
self.default_torso_height = rospy.get_param('~default_torso_height',
0.2)
self.gripper_raise_dist = rospy.get_param(
'~graipper_post_push_raise_dist', 0.05)
use_slip = rospy.get_param('~use_slip_detection', 1)
self.tf_listener = tf.TransformListener()
# Set joint gains
prefix = roslib.packages.get_pkg_dir('hrl_pr2_arms') + '/params/'
cs = ControllerSwitcher()
rospy.loginfo(
cs.switch("r_arm_controller", "r_arm_controller",
prefix + "pr2_arm_controllers_push.yaml"))
rospy.loginfo(
cs.switch("l_arm_controller", "l_arm_controller",
prefix + "pr2_arm_controllers_push.yaml"))
# Setup arms
rospy.loginfo('Creating pr2 object')
self.robot = pr2.PR2(self.tf_listener, arms=True, base=False)
rospy.loginfo('Setting up left arm move')
self.left_arm_move = lm.LinearReactiveMovement('l', self.robot,
self.tf_listener,
use_slip, use_slip)
rospy.loginfo('Setting up right arm move')
self.right_arm_move = lm.LinearReactiveMovement(
'r', self.robot, self.tf_listener, use_slip, use_slip)
self.push_pose_proxy = rospy.ServiceProxy('get_push_pose', PushPose)
self.gripper_push_service = rospy.Service('gripper_push', GripperPush,
self.gripper_push_action)
self.gripper_pre_push_service = rospy.Service(
'gripper_pre_push', GripperPush, self.gripper_pre_push_action)
self.gripper_post_push_service = rospy.Service(
'gripper_post_push', GripperPush, self.gripper_post_push_action)
self.gripper_sweep_service = rospy.Service('gripper_sweep',
GripperPush,
self.gripper_sweep_action)
self.gripper_pre_sweep_service = rospy.Service(
'gripper_pre_sweep', GripperPush, self.gripper_pre_sweep_action)
self.gripper_post_sweep_service = rospy.Service(
'gripper_post_sweep', GripperPush, self.gripper_post_sweep_action)
self.overhead_push_service = rospy.Service('overhead_push',
GripperPush,
self.overhead_push_action)
self.overhead_pre_push_service = rospy.Service(
'overhead_pre_push', GripperPush, self.overhead_pre_push_action)
self.overhead_post_push_service = rospy.Service(
'overhead_post_push', GripperPush, self.overhead_post_push_action)
self.raise_and_look_serice = rospy.Service('raise_and_look',
RaiseAndLook,
self.raise_and_look_action)
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
#joint_controller = EPArmController(joint_arm, 0.1, "joint_ep_controller")
#joint_controller.execute_interpolated_ep(setup_angles, 10)
ctrl_switcher.carefree_switch('l', '%s_cart', None)
rospy.sleep(1)
cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
cart_arm.set_posture(setup_angles)
#cart_arm.set_posture(cart_arm.get_joint_angles(wrapped=False))
cart_arm.set_gains([110, 600, 600, 40, 40, 40], [15, 15, 15, 1.2, 1.2, 1.2])
ell_controller = EllipsoidController(cart_arm)
ell_controller.reset_arm_orientation(8)
pg.init()
screen = pg.display.set_mode((300,300))
while not rospy.is_shutdown():
pg.event.get()
keys = pg.key.get_pressed()
dur = 5
if True:
r = np.random.randint(6)
if r == 0:
ell_controller.command_move([ 1, 0, 0], dur)
elif r == 1:
ell_controller.command_move([-1, 0, 0], dur)
elif r == 2:
ell_controller.command_move([ 0, 1, 0], dur)
elif r == 3:
ell_controller.command_move([ 0, -1, 0], dur)
elif r == 4:
ell_controller.command_move([ 0, 0, 1], dur)
elif r == 5:
ell_controller.command_move([ 0, 0, -1], dur)
rospy.sleep(1)
else:
if keys[pg.K_w]:
ell_controller.command_move([-1, 0, 0], dur)
if keys[pg.K_s]:
ell_controller.command_move([ 1, 0, 0], dur)
if keys[pg.K_a]:
ell_controller.command_move([ 0, 1, 0], dur)
if keys[pg.K_d]:
ell_controller.command_move([ 0, -1, 0], dur)
if keys[pg.K_q]:
ell_controller.command_move([ 0, 0, 1], dur)
if keys[pg.K_e]:
ell_controller.command_move([ 0, 0, -1], dur)
rospy.sleep(0.05)
def __init__(self):
self.ctrl_switcher = ControllerSwitcher()
self.torso_sac = actionlib.SimpleActionClient('torso_controller/position_joint_action',
SingleJointPositionAction)
self.torso_sac.wait_for_server()
self.head_point_sac = actionlib.SimpleActionClient(
'/head_traj_controller/point_head_action',
PointHeadAction)
self.head_point_sac.wait_for_server()
rospy.loginfo("[experiment_setup] ExperimentSetup ready.")
def main():
rospy.init_node("carefree_switch_controller", sys.argv)
if len(sys.argv) <= 1 or sys.argv[1] in ["-h", "--help"]:
print "Usage: arm_side new_controller '[param_file]'"
return
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
param_path = sys.argv[3]
if 'find' not in param_path:
param_path = "$(find hrl_pr2_arms)/params/" + param_path
print cs.carefree_switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.carefree_switch(sys.argv[1], sys.argv[2])
def main():
rospy.init_node("switch_controller", sys.argv)
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
if len(sys.argv) >= 5:
pkg = sys.argv[3]
param_file = sys.argv[4]
else:
pkg = "hrl_pr2_arms"
param_file = sys.argv[3]
param_path = "$(find %s)/params/%s" % (pkg, param_file)
print cs.switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.switch(sys.argv[1], sys.argv[2])
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(
'r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch(
'r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r',
PR2ArmJointTrajectory,
controller_name='%s_joint_controller_low')
q = [
-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314,
-1.82146607, -1.85187734
]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch(
'r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi / 2)[:3, :3]
vel_ctrl.move_velocity(velocity_rot=vel_frame,
velocity=0.10,
is_translation=False)
def main():
rospy.init_node("arm_cart_control_backend")
ctrl_switcher = ControllerSwitcher()
if False:
ctrl_switcher.carefree_switch(
'r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch(
'r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r',
PR2ArmJointTrajectory,
controller_name='%s_joint_controller_low')
q = [
-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314,
-1.82146607, -1.85187734
]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch(
'r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r',
PR2ArmCartesianPostureBase,
controller_name='%s_cart_low_rfh')
r_arm.set_posture()
rospy.sleep(0.2)
l_arm = None
#l_arm = create_pr2_arm('l', PR2ArmCartesianPostureBase)
#l_arm.set_posture()
rospy.sleep(0.2)
cart_ctrl = ArmCartCtrlBackend(r_arm, l_arm, MONITOR_RATE)
cart_ctrl.backend_loop()
def setup_servoing_arms(msg):
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('r', joint_ctrl, reset=False)
ctrl_switcher.carefree_switch('l', joint_ctrl, reset=False)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name=joint_ctrl)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory, controller_name=joint_ctrl)
r_ep_arm_ctrl = EPArmController(r_arm)
l_ep_arm_ctrl = EPArmController(l_arm)
r_ep_arm_ctrl.execute_interpolated_ep([-1.91, 1.25, -1.93, -1.53, 0.33, -0.03, 0.0],
15, blocking=False)
l_ep_arm_ctrl.execute_interpolated_ep([1.91, 1.25, 1.93, -1.53, -0.33, -0.03, -3.09],
15, blocking=True)
return EmptyResponse()
class SetupTaskController(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
def execute(self, userdata):
self.ctrl_switcher.carefree_switch('l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
rospy.sleep(1)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
self.arm.set_posture(setup_angles)
self.arm.set_gains([200, 800, 800, 80, 80, 80], [15, 15, 15, 1.2, 1.2, 1.2])
return 'succeeded'
def __init__(self):
self.ellipsoid = EllipsoidSpace()
self.controller_switcher = ControllerSwitcher()
self.ee_frame = '/l_gripper_shaver305_frame'
self.head_pose = None
self.tfl = TransformListener()
self.is_forced_retreat = False
self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array', PoseArray)
self.global_input_sub = rospy.Subscriber("/face_adls/global_move", String,
async_call(self.global_input_cb))
self.local_input_sub = rospy.Subscriber("/face_adls/local_move", String,
async_call(self.local_input_cb))
self.clicked_input_sub = rospy.Subscriber("/face_adls/clicked_move", PoseStamped,
async_call(self.global_input_cb))
self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
self.global_move_poses_pub = rospy.Publisher('/face_adls/global_move_poses',
StringArray, latch=True)
self.controller_enabled_pub = rospy.Publisher('/face_adls/controller_enabled',
Bool, latch=True)
self.enable_controller_srv = rospy.Service("/face_adls/enable_controller",
EnableFaceController, self.enable_controller_cb)
self.request_registration = rospy.ServiceProxy("/request_registration", RequestRegistration)
self.enable_mpc_srv = rospy.ServiceProxy('/haptic_mpc/enable_mpc', EnableHapticMPC)
self.ell_params_pub = rospy.Publisher("/ellipsoid_params", EllipsoidParams, latch=True)
def stop_move_cb(msg):
self.stop_moving()
self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move", Bool, stop_move_cb, queue_size=1)
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber("/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber("/face_adls/%s_cart_absolute" % arm_char,
PoseStamped, async_call(self.command_absolute_cb))
self.adjust_posture_sub = rospy.Subscriber("/face_adls/%s_cart_posture" % arm_char,
Float32, self.adjust_posture_cb)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot([0]*3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name,
frame_rot, velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
self.controller_enabled_pub = rospy.Publisher('/face_adls/%s_cart_ctrl_enabled' % arm_char,
Bool, latch=True)
self.enable_controller_srv = rospy.Service("/face_adls/%s_enable_cart_ctrl" % arm_char,
EnableCartController, enable_controller_cb)
def __init__(self, arm_char, ctrl_name="%s_arm_controller", param_file=None):
self.arm_char = arm_char
self.ctrl_name = ctrl_name
self.param_file = param_file
self.cur_joint_traj = None
self.running = False
self.is_paused = False
self.ctrl_switcher = ControllerSwitcher()
def main():
rospy.init_node("carefree_switch_controller", sys.argv)
if len(sys.argv) <= 1:
print "Usage: arm new_controller [param_file]"
return
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
if len(sys.argv) >= 5:
pkg = sys.argv[3]
param_file = sys.argv[4]
else:
pkg = 'hrl_pr2_arms'
param_file = sys.argv[3]
param_path = "$(find %s)/params/%s" % (pkg, param_file)
print cs.carefree_switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.carefree_switch(sys.argv[1], sys.argv[2])
def main():
rospy.init_node("arm_cart_control_backend")
ctrl_switcher = ControllerSwitcher()
if False:
ctrl_switcher.carefree_switch('r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch('r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase, controller_name='%s_cart_low_rfh')
r_arm.set_posture()
rospy.sleep(0.2)
l_arm = None
#l_arm = create_pr2_arm('l', PR2ArmCartesianPostureBase)
#l_arm.set_posture()
rospy.sleep(0.2)
cart_ctrl = ArmCartCtrlBackend(r_arm, l_arm, MONITOR_RATE)
cart_ctrl.backend_loop()
def __init__(self, arm_char, ctrl_name="%s_arm_controller", param_file=None):
self.arm_char = arm_char
self.ctrl_name = ctrl_name
self.param_file = param_file
self.cur_joint_traj = None
self.running = False
self.is_paused = False
self.ctrl_switcher = ControllerSwitcher()
rospy.loginfo('[trajectory_playback] Started Traj Playback for %s arm with %s controller.' %(self.arm_char, self.ctrl_name))
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
ctrl_switcher.carefree_switch('r', '%s_arm_controller', None)
rospy.sleep(1)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
l_tuck_angles = [ 0.14228106, 1.29643293, 1.78480255, -1.56470338, 1.16505304,
-0.09788312, 0.23542476]
r_tuck_angles = [ 0.01289596, 1.02437885, -1.34551339, -1.78272859, 0.38331793,
-1.28334274, 0.02605728]
l_joint_controller = EPArmController(l_arm, 0.1, "l_joint_ep_controller")
r_joint_controller = EPArmController(r_arm, 0.1, "r_joint_ep_controller")
l_joint_controller.execute_interpolated_ep(l_tuck_angles, 10)
r_joint_controller.execute_interpolated_ep(r_tuck_angles, 10)
def __init__(self):
self.ellipsoid = EllipsoidSpace()
self.controller_switcher = ControllerSwitcher()
self.ee_frame = '/l_gripper_shaver305_frame'
self.head_pose = None
self.tfl = TransformListener()
self.is_forced_retreat = False
self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array',
PoseArray)
self.global_input_sub = rospy.Subscriber(
"/face_adls/global_move", String, async_call(self.global_input_cb))
self.local_input_sub = rospy.Subscriber(
"/face_adls/local_move", String, async_call(self.local_input_cb))
self.clicked_input_sub = rospy.Subscriber(
"/face_adls/clicked_move", PoseStamped,
async_call(self.global_input_cb))
self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
self.global_move_poses_pub = rospy.Publisher(
'/face_adls/global_move_poses', StringArray, latch=True)
self.controller_enabled_pub = rospy.Publisher(
'/face_adls/controller_enabled', Bool, latch=True)
self.enable_controller_srv = rospy.Service(
"/face_adls/enable_controller", EnableFaceController,
self.enable_controller_cb)
self.request_registration = rospy.ServiceProxy("/request_registration",
RequestRegistration)
self.enable_mpc_srv = rospy.ServiceProxy('/haptic_mpc/enable_mpc',
EnableHapticMPC)
self.ell_params_pub = rospy.Publisher("/ellipsoid_params",
EllipsoidParams,
latch=True)
def stop_move_cb(msg):
self.stop_moving()
self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move",
Bool,
stop_move_cb,
queue_size=1)
class SetupTaskController(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
def execute(self, userdata):
self.ctrl_switcher.carefree_switch(
'l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
rospy.sleep(1)
setup_angles = [
0, 0, np.pi / 2, -np.pi / 2, -np.pi, -np.pi / 2, -np.pi / 2
]
self.arm.set_posture(setup_angles)
self.arm.set_gains([200, 800, 800, 80, 80, 80],
[15, 15, 15, 1.2, 1.2, 1.2])
return 'succeeded'
def move(self):
# this is for robot_mechanism_controllers/CartesianTwistController
ctrl_switcher = ControllerSwitcher()
initialPose(ctrl_switcher)
rospy.loginfo('create the controller')
ctrl_switcher.carefree_switch('r', '%s_cart', '$(find visual_servo)/params/rmc_cartTwist_params.yaml')
rospy.sleep(0.5)
pub = rospy.Publisher('r_cart/command', Twist)
rospy.loginfo('created the publisher obj')
pose = Twist()
pose.linear.x = 0.00
pose.linear.y = 0.00
pose.linear.z = -.05
pose.angular.x = 0.1
pose.angular.y = 0.1
pose.angular.z = 0.1
while not rospy.is_shutdown():
rospy.loginfo('Publishing following message: %s'%pose)
pub.publish(pose)
rospy.sleep(2.0)
def main():
rospy.init_node("ellipsoidal_controller_backend")
cart_arm = create_ep_arm('l', PR2ArmJTransposeTask,
controller_name='%s_cart_jt_task',
end_link="%s_gripper_shaver45_frame")
ell_backend = EllipsoidalInterfaceBackend(cart_arm)
ell_backend.disable_interface("Setting up arm.")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_ep_arm('l', PR2ArmJointTraj)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
joint_arm.set_ep(setup_angles, 5)
rospy.sleep(5)
ctrl_switcher.carefree_switch('l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
rospy.sleep(1)
if True:
rospy.sleep(1)
setup_angles = [0, 0, np.pi/2, -np.pi/2, -np.pi, -np.pi/2, -np.pi/2]
cart_arm.set_posture(setup_angles)
cart_arm.set_gains([200, 800, 800, 80, 80, 80], [15, 15, 15, 1.2, 1.2, 1.2])
ell_backend.controller.reset_arm_orientation(8)
ell_backend.enable_interface("Controller ready.")
rospy.spin()
ell_backend.print_statistics()
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
ctrl_switcher.carefree_switch('r', '%s_arm_controller', None)
rospy.sleep(1)
l_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
l_tuck_angles = [
0.14228106, 1.29643293, 1.78480255, -1.56470338, 1.16505304,
-0.09788312, 0.23542476
]
r_tuck_angles = [
0.01289596, 1.02437885, -1.34551339, -1.78272859, 0.38331793,
-1.28334274, 0.02605728
]
l_joint_controller = EPArmController(l_arm, 0.1, "l_joint_ep_controller")
r_joint_controller = EPArmController(r_arm, 0.1, "r_joint_ep_controller")
l_joint_controller.execute_interpolated_ep(l_tuck_angles, 10)
r_joint_controller.execute_interpolated_ep(r_tuck_angles, 10)
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber(
"/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber(
"/face_adls/%s_cart_absolute" % arm_char, PoseStamped,
async_call(self.command_absolute_cb))
self.adjust_posture_sub = rospy.Subscriber(
"/face_adls/%s_cart_posture" % arm_char, Float32,
self.adjust_posture_cb)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot(
[0] * 3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link,
req.ctrl_params,
req.ctrl_name,
frame_rot,
velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
self.controller_enabled_pub = rospy.Publisher(
'/face_adls/%s_cart_ctrl_enabled' % arm_char, Bool, latch=True)
self.enable_controller_srv = rospy.Service(
"/face_adls/%s_enable_cart_ctrl" % arm_char, EnableCartController,
enable_controller_cb)
def main():
rospy.init_node("switch_controller", sys.argv)
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
if len(sys.argv) >= 5:
pkg = sys.argv[3]
param_file = sys.argv[4]
else:
pkg = 'hrl_pr2_arms'
param_file = sys.argv[3]
param_path = "$(find %s)/params/%s" % (pkg, param_file)
print cs.switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.switch(sys.argv[1], sys.argv[2])
def __init__(self, arm):
self.time_step = 1. / 20.
self.gripper_rot = np.pi
self.arm = arm
self.ell_traj_behavior = EPC("ellipsoid_traj")
self.ell_space = EllipsoidSpace(1)
self.tf_list = tf.TransformListener()
self.found_params = False
self.ell_sub = rospy.Subscriber("/ellipsoid_params", EllipsoidParams,
self.read_params)
self.start_pub = rospy.Publisher("/start_pose", PoseStamped)
self.end_pub = rospy.Publisher("/end_pose", PoseStamped)
self.ctrl_switcher = ControllerSwitcher()
self.ell_cmd_lock = Lock()
self.params_lock = Lock()
self.ell_ep = None
self.action_preempted = False
self.ell_move_act = actionlib.SimpleActionServer(
"/ellipsoid_move", EllipsoidMoveAction, self.command_move_exec,
False)
self.ell_move_act.start()
def main():
rospy.init_node("carefree_switch_controller", sys.argv)
if len(sys.argv) <= 1:
print "Usage: arm new_controller [param_file]"
return
cs = ControllerSwitcher()
if len(sys.argv) >= 4:
if len(sys.argv) >= 5:
pkg = sys.argv[3]
param_file = sys.argv[4]
else:
pkg = 'hrl_pr2_arms'
param_file = sys.argv[3]
param_path = "$(find %s)/params/%s" % (pkg, param_file)
print cs.carefree_switch(sys.argv[1], sys.argv[2], param_path)
else:
print cs.carefree_switch(sys.argv[1], sys.argv[2])
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch('r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi/2)[:3,:3]
vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.10, is_translation=False)
def main():
rospy.init_node("test_ellipsoid_controller")
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = [
0, 0, np.pi / 2, -np.pi / 2, -np.pi, -np.pi / 2, -np.pi / 2
]
#joint_controller = EPArmController(joint_arm, 0.1, "joint_ep_controller")
#joint_controller.execute_interpolated_ep(setup_angles, 10)
ctrl_switcher.carefree_switch('l', '%s_cart', None)
rospy.sleep(1)
cart_arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
cart_arm.set_posture(setup_angles)
#cart_arm.set_posture(cart_arm.get_joint_angles(wrapped=False))
cart_arm.set_gains([110, 600, 600, 40, 40, 40],
[15, 15, 15, 1.2, 1.2, 1.2])
ell_controller = EllipsoidController(cart_arm)
ell_controller.reset_arm_orientation(8)
pg.init()
screen = pg.display.set_mode((300, 300))
while not rospy.is_shutdown():
pg.event.get()
keys = pg.key.get_pressed()
dur = 5
if True:
r = np.random.randint(6)
if r == 0:
ell_controller.command_move([1, 0, 0], dur)
elif r == 1:
ell_controller.command_move([-1, 0, 0], dur)
elif r == 2:
ell_controller.command_move([0, 1, 0], dur)
elif r == 3:
ell_controller.command_move([0, -1, 0], dur)
elif r == 4:
ell_controller.command_move([0, 0, 1], dur)
elif r == 5:
ell_controller.command_move([0, 0, -1], dur)
rospy.sleep(1)
else:
if keys[pg.K_w]:
ell_controller.command_move([-1, 0, 0], dur)
if keys[pg.K_s]:
ell_controller.command_move([1, 0, 0], dur)
if keys[pg.K_a]:
ell_controller.command_move([0, 1, 0], dur)
if keys[pg.K_d]:
ell_controller.command_move([0, -1, 0], dur)
if keys[pg.K_q]:
ell_controller.command_move([0, 0, 1], dur)
if keys[pg.K_e]:
ell_controller.command_move([0, 0, -1], dur)
rospy.sleep(0.05)
class TrajPlayback(object):
##
# @param arm_char 'r' or 'l' to choose which arm to load
# @param ctrl_name name of the controller to load from the param_file
# @param param_file Location of the parameter file which specifies the controller
# (e.g. "$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml")
def __init__(self, arm_char, ctrl_name="%s_arm_controller", param_file=None):
self.arm_char = arm_char
self.ctrl_name = ctrl_name
self.param_file = param_file
self.cur_joint_traj = None
self.running = False
self.is_paused = False
self.ctrl_switcher = ControllerSwitcher()
#rospy.loginfo('[trajectory_playback] Started Traj Playback for %s arm with %s controller.' %(self.arm_char, self.ctrl_name))
##
# Switches controllers and returns an instance of the arm to control
def load_arm(self):
self.ctrl_switcher.carefree_switch(self.arm_char, self.ctrl_name,
self.param_file, reset=False)
return create_ep_arm(self.arm_char, PR2ArmJointTraj,
controller_name=self.ctrl_name, timeout=8)
##
# Plays back the specified trajectory. The arm must currently be at the first
# joint configuration specified in the joint trajectory within a certain degree of
# tolerance.
# @param joint_trajectory List of lists of length 7 representing joint angles to move through.
# @param rate Frequency with which to iterate through the list.
# @param blocking If True, the function will wait until done, if False, it will return
# immediately
def execute(self, joint_trajectory, rate, blocking=True):
if len(joint_trajectory) == 0:
return True
if self.running:
rospy.logerr("[arm_pose_move_controller] Trajectory already in motion.")
return False
self.cur_arm = self.load_arm()
if not self.can_exec_traj(joint_trajectory):
rospy.logwarn("[arm_pose_move_controller] Arm not at trajectory start.")
return True
def exec_traj(te):
self.cur_idx = 0
self.stop_traj = False
self.is_paused = False
self.running = True
rospy.loginfo("[arm_pose_move_controller] Starting trajectory.")
r = rospy.Rate(rate)
global stop
while (not rospy.is_shutdown() and
not self.stop_traj and
self.cur_idx < len(joint_trajectory) and
stop != "STOP"):
self.cur_arm.set_ep(joint_trajectory[self.cur_idx], 1./rate)
self.cur_idx += 1
r.sleep()
if stop == "STOP":
self.stop_moving()
self.stop_traj = True
return
self.cur_arm.set_ep(self.cur_arm.get_ep(), 0.3)
if self.cur_idx < len(joint_trajectory):
self.cur_joint_traj = joint_trajectory[self.cur_idx:]
successful = False
else:
self.cur_joint_traj = None
self.is_paused = False
successful = True
self.last_rate = rate
self.running = False
return successful
if blocking:
return exec_traj(None)
else:
self.exec_traj_timer = rospy.Timer(rospy.Duration(0.01), exec_traj, oneshot=True)
return True
##
# Executes a linearly interpolated trajectory from the current joint angles to the
# q_goal angles.
# @param q_goal List of length 7 representing the desired end configuration.
# @param rate Rate with which to execute trajectory.
# @param velocity Speed (~rad/s) to move based on a heursitic which weighs relative
# joint speeds. The elbow will not move quicker than the velocity
# in rad/s.
# @param blocking If True, the function will wait until done, if False, it will return
# immediately
def move_to_angles(self, q_goal, rate=RATE, velocity=0.1, blocking=True):
traj = self.get_angle_traj(q_goal, rate, velocity)
if len(traj) == 0:
return True
return self.execute(traj, rate, blocking)
def get_angle_traj(self, q_goal, rate=RATE, velocity=0.1):
self.cur_arm = self.load_arm()
q_cur = self.cur_arm.get_ep()
diff = self.cur_arm.difference_joints(q_goal, q_cur)
max_ang = np.max(np.fabs(diff) * JOINT_VELOCITY_WEIGHT)
time_traj = max_ang / velocity
steps = np.round(rate * time_traj)
if steps == 0:
return []
t_vals = np.linspace(0., 1., steps)
return [q_cur + diff * t for t in t_vals]
##
# Determines whether or not the arm can execute the trajectory by checking the first
# joint configuration and seeing whether or not it is within joint tolerances.
# @param joint_trajectory List of lists of length 7 representing joint angles to move through.
# @return True if the arm is at the beginning, False otherwise.
def can_exec_traj(self, joint_trajectory):
if len(joint_trajectory) == 0:
return True
q_cur = self.cur_arm.get_ep()
q_init = joint_trajectory[0]
diff = self.cur_arm.difference_joints(q_cur, q_init)
return np.all(np.fabs(diff) < JOINT_TOLERANCES)
def is_moving(self):
return self.running
##
# Pauses the movement of the trajectory but doesn't reset its position in the array.
def pause_moving(self):
if self.is_moving():
self.stop_traj = True
while not rospy.is_shutdown() and self.running:
rospy.sleep(0.01)
self.is_paused = True
return True
return False
##
# Restarts the currently running movement after being paused.
def restart_moving(self, blocking=True):
if not self.is_paused or self.cur_joint_traj is None:
return False
self.execute(self.cur_joint_traj, self.last_rate, blocking=blocking)
return True
##
# Stops the movement of the trajectory and resets the trajectory so it cannot restart.
def stop_moving(self):
self.stop_traj = True
self.is_paused = False
self.cur_joint_traj = None
while not rospy.is_shutdown() and self.running:
rospy.sleep(0.01)
def preempt(self):
self.stop_moving()
def run(data):
action = data.data
print ("Traj playback heard: %s, starting." % action)
task_set = rospy.Publisher('task_check', String)
task_set.publish("Starting %s" %action)
#Not important, not running from command line
from optparse import OptionParser
p = OptionParser()
p.add_option('-f', '--filename', dest="filename", default="",
help="File to save trajectory to or load from.")
p.add_option('-l', '--left_arm', dest="left_arm",
action="store_true", default=False,
help="Use left arm.")
p.add_option('-r', '--right_arm', dest="right_arm",
action="store_true", default=False,
help="Use right arm.")
p.add_option('-s', '--save_mode', dest="save_mode",
action="store_true", default=False,
help="Saving mode.")
p.add_option('-t', '--traj_mode', dest="traj_mode",
action="store_true", default=False,
help="Trajectory mode.")
p.add_option('-c', '--ctrl_name', dest="ctrl_name", default=CTRL_NAME_LOW,
help="Controller to run the playback with.")
p.add_option('-p', '--params', dest="param_file", default=PARAM_FILE_LOW, #None,
help="YAML file to save parameters in or load from.")
p.add_option('-v', '--srv_mode', dest="srv_mode",
action="store_true", default=False,
help="Server mode.")
p.add_option('-y', '--playback_mode', dest="playback_mode",
action="store_true", default=False,
help="Plays back trajectory immediately.")
p.add_option('-z', '--reverse', dest="reverse",
action="store_true", default=False,
help="Plays back trajectory in reverse.")
(opts, args) = p.parse_args()
#Selects arm and action based on action taken from msg string
if opts.right_arm:
arm_char = 'r'
elif opts.left_arm:
arm_char = 'l'
else:
print "Traj playback selecting default left arm."
arm_char = 'l'
if action == "GoToHome":
opts.traj_mode = False
opts.playback_mode = True
filename = 'GoToHome'
elif action == "HomeToBowl":
opts.traj_mode = False
opts.playback_mode = True
filename = 'HomeToBowl'
elif action == "ScoopYogurt":
opts.traj_mode = False
opts.playback_mode = True
filename = 'ScoopYogurt'
elif action == "MoveToCheck":
opts.traj_mode = False
opts.playback_mode = True
filename = 'MoveToCheck'
elif action == "SwitchControllers":
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(arm_char, CTRL_NAME_DEFAULT, None, reset=False)
print "Controller returned to default"
opts.traj_mode = False
opts.playback_mode = False
task_set = rospy.Publisher('task_check', String)
task_return = action + "Done"
task_set.publish(task_return)
running = False
return
elif opts.filename == "":
print ("Heard: %s, will keep listening." % action)
running = False
return
opts.filename = filename
#Traj playback action is called here. For Yogurt delivery only playback_mode is needed
if opts.save_mode:
assert(opts.right_arm + opts.left_arm == 1)
if opts.traj_mode:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(arm_char, CTRL_NAME_NONE, PARAM_FILE_NONE, reset=False)
traj_saver = TrajectorySaver(RATE, CTRL_NAME_NONE)
raw_input("Press enter to start recording")
traj_saver.record_trajectory(arm_char, blocking=False)
import curses
def wait_for_key(stdscr):
curses.init_pair(1, curses.COLOR_RED, curses.COLOR_WHITE)
stdscr.addstr(0,0, "Press any key to stop!", curses.color_pair(1) )
stdscr.refresh()
c = 0
while not rospy.is_shutdown() and c == 0:
c = stdscr.getch()
curses.wrapper(wait_for_key)
traj_saver.stop_record(roslaunch.substitution_args.resolve_args(opts.filename))
ctrl_switcher.carefree_switch(arm_char, opts.ctrl_name, PARAM_FILE_LOW, reset=False)
return
else:
print "FIX"
return
elif opts.srv_mode:
traj_srv = TrajectoryServer(arm_char, "/trajectory_playback_" + arm_char,
opts.ctrl_name, opts.param_file)
rospy.spin()
return
#Should only run in playback_mode
elif opts.playback_mode:
print ("starting to perform traj action for: " + action)
if opts.traj_mode:
exec_traj_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
#Should not run in traj_mode
else:
print "Beginning moving to trajectory start"
move_to_setup_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
print "Finished moving to trajectory start"
print "Beginning trajectory"
exec_traj_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
print "Done with playback"
task_set = rospy.Publisher('task_check', String)
task_return = action + "Done"
task_set.publish(task_return)
print "finished trajectory: ", task_return
action = ""
return
def __init__(self):
self.tf = TransformListener()
self.jtarms = jttask_utils.jt_task_utils(self.tf)
self.controller_switcher = ControllerSwitcher()
##### Subscribers ####
self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8,
self.disc_change_pose)
self.disc_sub.impl.add_callback(self.cancel_goals, None)
self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point,
self.cont_change_pose)
self.cont_sub.impl.add_callback(self.cancel_goals, None)
self.loc_sub = rospy.Subscriber('wt_shave_location', Int8,
self.set_shave_pose)
self.loc_sub.impl.add_callback(self.cancel_goals, None)
rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving)
#### Services ####
rospy.loginfo("Waiting for get_head_pose service")
try:
rospy.wait_for_service('/get_head_pose', 5.0)
self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose',
GetHeadPose)
rospy.loginfo("Found get_head_pose service")
except:
rospy.logwarn("get_head_pose service not available")
rospy.loginfo("Waiting for ellipsoid_command service")
try:
rospy.wait_for_service('/ellipsoid_command', 5.0)
self.ellipsoid_command_proxy = rospy.ServiceProxy(
'/ellipsoid_command', EllipsoidCommand)
rospy.loginfo("Found ellipsoid_command service")
except:
rospy.logwarn("ellipsoid_command service not available")
#### Publishers ####
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_out = rospy.Publisher('test_pose_1', PoseStamped)
self.rezero_wrench = rospy.Publisher(
'/netft_gravity_zeroing/rezero_wrench', Bool)
#### Data ####
self.hand_offset = 0.195
self.angle_tool_offset = 0.03
self.inline_tool_offset = 0.085
self.selected_pose = 0
self.poses = {
0: ["near_ear", 0.],
1: ["upper_cheek", 0.],
2: ["middle_cheek", 0.],
3: ["jaw_bone", 0.],
4: ["back_neck", 0.],
5: ["nose", 0.],
6: ["chin", 0.],
7: ["mouth_corner", 0.]
}
#self.ft_wrench = WrenchStamped()
# self.force_unsafe = False
# self.ft_activity_thresh = 3
# self.ft_safety_thresh = 12
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
# rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state)
if POSTURE:
rospy.sleep(3)
print 'Sending Posture'
self.jtarms.send_posture(posture='elbowup', arm=0)
class Shaving_manager():
def __init__(self):
self.tf = TransformListener()
self.jtarms = jttask_utils.jt_task_utils(self.tf)
self.controller_switcher = ControllerSwitcher()
##### Subscribers ####
self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8,
self.disc_change_pose)
self.disc_sub.impl.add_callback(self.cancel_goals, None)
self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point,
self.cont_change_pose)
self.cont_sub.impl.add_callback(self.cancel_goals, None)
self.loc_sub = rospy.Subscriber('wt_shave_location', Int8,
self.set_shave_pose)
self.loc_sub.impl.add_callback(self.cancel_goals, None)
rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving)
#### Services ####
rospy.loginfo("Waiting for get_head_pose service")
try:
rospy.wait_for_service('/get_head_pose', 5.0)
self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose',
GetHeadPose)
rospy.loginfo("Found get_head_pose service")
except:
rospy.logwarn("get_head_pose service not available")
rospy.loginfo("Waiting for ellipsoid_command service")
try:
rospy.wait_for_service('/ellipsoid_command', 5.0)
self.ellipsoid_command_proxy = rospy.ServiceProxy(
'/ellipsoid_command', EllipsoidCommand)
rospy.loginfo("Found ellipsoid_command service")
except:
rospy.logwarn("ellipsoid_command service not available")
#### Publishers ####
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_out = rospy.Publisher('test_pose_1', PoseStamped)
self.rezero_wrench = rospy.Publisher(
'/netft_gravity_zeroing/rezero_wrench', Bool)
#### Data ####
self.hand_offset = 0.195
self.angle_tool_offset = 0.03
self.inline_tool_offset = 0.085
self.selected_pose = 0
self.poses = {
0: ["near_ear", 0.],
1: ["upper_cheek", 0.],
2: ["middle_cheek", 0.],
3: ["jaw_bone", 0.],
4: ["back_neck", 0.],
5: ["nose", 0.],
6: ["chin", 0.],
7: ["mouth_corner", 0.]
}
#self.ft_wrench = WrenchStamped()
# self.force_unsafe = False
# self.ft_activity_thresh = 3
# self.ft_safety_thresh = 12
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
# rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state)
if POSTURE:
rospy.sleep(3)
print 'Sending Posture'
self.jtarms.send_posture(posture='elbowup', arm=0)
# def get_ft_state(self, ws):
# self.ft_wrench = ws
# self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
# if self.ft_mag > self.ft_safety_thresh:
# self.force_unsafe = True
# if self.ft_mag > self.ft_activity_thresh:
# self.ft_activity = rospy.Time.now()
def stop_shaving(self, msg):
self.cancel_goals(msg)
rospy.loginfo("Stopping Shaving Behavior")
self.wt_log_out.publish(data="Stopping Shaving Behavior")
self.controller_switcher.carefree_switch('l', '%s_arm_controller')
def cancel_goals(self, msg):
self.jtarms.ft_move_client.cancel_all_goals(
) #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
rospy.loginfo("Cancelling Active Shaving Goals")
self.wt_log_out.publish(data="Cancelling Active Shaving Goals")
def cont_change_pose(self, step):
self.controller_switcher.carefree_switch('l', '%s_cart')
self.new_pose = True
req = EllipsoidCommandRequest()
req.change_latitude = int(-step.y)
req.change_longitude = int(-step.x)
req.change_height = int(step.z)
print req
print type(req.change_latitude)
self.ellipsoid_command_proxy.call(req)
rospy.loginfo("Moving Across Ellipsoid")
self.wt_log_out.publish("Moving Across Ellipsoid")
def disc_change_pose(self, step):
self.new_pose = True
self.selected_pose += step.data
if self.selected_pose > 7:
self.selected_pose = 7
self.wt_log_out.publish(
data="Final position in sequence, there is no next position")
return
elif self.selected_pose < 0:
self.selected_pose = 0
self.wt_log_out.publish(
data="First position in sequence, there is no previous position"
)
return
rospy.loginfo("Moving to " + self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to " +
self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(
self.poses[self.selected_pose][0],
self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def set_shave_pose(self, num):
self.selected_pose = num.data
rospy.loginfo("Moving to " + self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to " +
self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(
self.poses[self.selected_pose][0],
self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def adjust_pose(self, ellipse_pose):
self.controller_switcher.carefree_switch('l', '%s_cart')
self.jtarms.ft_move_client.cancel_all_goals(
) #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
#self.test_out.publish(ellipse_pose)
print "New Position Received, should stop current action"
self.tf.waitForTransform(ellipse_pose.header.frame_id,
'torso_lift_link', ellipse_pose.header.stamp,
rospy.Duration(3.0))
torso_pose = self.tf.transformPose('torso_lift_link', ellipse_pose)
if TOOL == 'inline':
goal_pose = self.jtarms.pose_frame_move(
torso_pose,
-(self.hand_offset + self.inline_tool_offset),
arm=0)
approach_pose = self.jtarms.pose_frame_move(goal_pose,
-0.15,
arm=0)
elif TOOL == '90':
goal_pose = self.jtarms.pose_frame_move(torso_pose, 0.015)
elif TOOL == '45':
goal_pose = torso_pose
approach_pose = self.jtarms.pose_frame_move(goal_pose,
-0.15,
arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose,
approach_pose,
arm=0)
self.ft_move(traj_to_approach)
self.rezero_wrench.publish(data=True)
self.ft_move(traj_appr_to_goal, ignore_ft=False)
retreat_traj = self.jtarms.build_trajectory(approach_pose, arm=0)
escape_traj = self.jtarms.build_trajectory(approach_pose,
arm=0,
steps=30)
self.jtarms.ft_hold_client.send_goal(
FtHoldGoal(2., 8, 10, rospy.Duration(
10))) #activity_thresh, z_unsafe, mag_unsafe, timeout
print "Waiting for hold result"
finished = self.jtarms.ft_hold_client.wait_for_result(
rospy.Duration(0))
print "Finished before Timeout: %s" % finished
print "Done Waiting"
hold_result = self.jtarms.ft_hold_client.get_result()
print "Hold Result:"
print hold_result
if hold_result.unsafe:
self.ft_move(escape_traj)
elif hold_result.timeout:
self.ft_move(retreat_traj)
def ft_move(self, traj, ft_thresh=2, ignore_ft=True):
self.jtarms.ft_move_client.send_goal(FtMoveGoal(
traj, ft_thresh, ignore_ft),
feedback_cb=self.ft_move_feedback)
finished_within_timeout = self.jtarms.ft_move_client.wait_for_result(
rospy.Duration(0.25 * len(traj)))
if finished_within_timeout:
result = self.jtarms.ft_move_client.get_result()
if result.all_sent:
rospy.loginfo("Full Trajectory Completed")
self.wt_log_out.publish(data="Full Trajectory Completed")
elif result.contact:
rospy.loginfo("Stopped Trajectory upon contact")
self.wt_log_out.publish(data="Stopped Trajectory upon contact")
else:
self.jtarms.ft_move_client.cancel_goal()
rospy.loginfo("Failed to complete movement within timeout period.")
self.wt_log_out.publish(
data="Failed to complete movement within timeout period.")
def ft_move_feedback(self, fdbk):
pct = 100. * float(fdbk.current) / float(fdbk.total)
#rospy.loginfo("Movement is %2.0f percent complete." %pct)
self.wt_log_out.publish(
data="Movement to %s is %2.0f percent complete." %
(self.poses[self.selected_pose][0], pct))
#def hold_ft_aware(self, escape_pose, arm=0):
# self.jtarms.force_stopped = False
# self.force_unsafe = False
# escape_traj = self.jtarms.build_trajectory(escape_pose)
# time_since_activity = rospy.Duration(0)
# self.ft_activity = rospy.Time.now()
# while not (rospy.is_shutdown() or self.force_unsafe or time_since_activity>rospy.Duration(10) or self.new_pose):
# time_since_activity = rospy.Time.now()-self.ft_activity
# rospy.sleep(0.01)
# if self.force_unsafe:
# rospy.loginfo("Unsafe High Forces, moving away!")
# self.wt_log_out.publish(data="Unsafe High Forces, moving away!")
# self.jtarms.goal_pub[arm].publish(escape_pose)
# return
# if time_since_activity>rospy.Duration(10):
# rospy.loginfo("10s of inactivity, moving away")
# self.wt_log_out.publish(data="10s of inactivity, moving away")
# if self.new_pose:
# rospy.loginfo("New Pose Received")
# self.wt_log_out.publish(data="New Pose Received")
# self.new_pose = False
# self.jtarms.send_traj(escape_traj)
def test(self):
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'torso_lift_link'
goal_pose.header.stamp = rospy.Time.now()
goal_pose.pose.position = Point(0.8, 0.3, 0.0)
goal_pose.pose.orientation = Quaternion(1, 0, 0, 0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose,
approach_pose,
arm=0)
raw_input("Move to approach pose")
self.jtarms.send_traj(traj_to_approach)
self.jtarms.force_stopped = False
self.force_unsafe = False
raw_input("Move to contact pose")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.jtarms.send_traj_to_contact(traj_appr_to_goal)
raw_input("Hold under force constraints")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.hold_ft_aware(approach_pose, arm=0)
rospy.loginfo("Finished Testing")
def __init__(self):
rospy.init_node('tabletop_push_node', log_level=rospy.DEBUG)
self.torso_z_offset = rospy.get_param('~torso_z_offset', 0.15)
self.look_pt_x = rospy.get_param('~look_point_x', 0.45)
self.head_pose_cam_frame = rospy.get_param('~head_pose_cam_frame',
'openni_rgb_frame')
self.default_torso_height = rospy.get_param('~default_torso_height',
0.2)
self.gripper_raise_dist = rospy.get_param('~graipper_post_push_raise_dist',
0.05)
use_slip = rospy.get_param('~use_slip_detection', 1)
self.tf_listener = tf.TransformListener()
# Set joint gains
prefix = roslib.packages.get_pkg_dir('hrl_pr2_arms')+'/params/'
cs = ControllerSwitcher()
rospy.loginfo(cs.switch("r_arm_controller", "r_arm_controller",
prefix + "pr2_arm_controllers_push.yaml"))
rospy.loginfo(cs.switch("l_arm_controller", "l_arm_controller",
prefix + "pr2_arm_controllers_push.yaml"))
# Setup arms
rospy.loginfo('Creating pr2 object')
self.robot = pr2.PR2(self.tf_listener, arms=True, base=False)
rospy.loginfo('Setting up left arm move')
self.left_arm_move = lm.LinearReactiveMovement('l', self.robot,
self.tf_listener,
use_slip, use_slip)
rospy.loginfo('Setting up right arm move')
self.right_arm_move = lm.LinearReactiveMovement('r', self.robot,
self.tf_listener,
use_slip, use_slip)
self.push_pose_proxy = rospy.ServiceProxy('get_push_pose', PushPose)
self.gripper_push_service = rospy.Service('gripper_push',
GripperPush,
self.gripper_push_action)
self.gripper_pre_push_service = rospy.Service('gripper_pre_push',
GripperPush,
self.gripper_pre_push_action)
self.gripper_post_push_service = rospy.Service('gripper_post_push',
GripperPush,
self.gripper_post_push_action)
self.gripper_sweep_service = rospy.Service('gripper_sweep',
GripperPush,
self.gripper_sweep_action)
self.gripper_pre_sweep_service = rospy.Service('gripper_pre_sweep',
GripperPush,
self.gripper_pre_sweep_action)
self.gripper_post_sweep_service = rospy.Service('gripper_post_sweep',
GripperPush,
self.gripper_post_sweep_action)
self.overhead_push_service = rospy.Service('overhead_push',
GripperPush,
self.overhead_push_action)
self.overhead_pre_push_service = rospy.Service('overhead_pre_push',
GripperPush,
self.overhead_pre_push_action)
self.overhead_post_push_service = rospy.Service('overhead_post_push',
GripperPush,
self.overhead_post_push_action)
self.raise_and_look_serice = rospy.Service('raise_and_look',
RaiseAndLook,
self.raise_and_look_action)
class FaceADLsManager(object):
def __init__(self):
self.ellipsoid = EllipsoidSpace()
self.controller_switcher = ControllerSwitcher()
self.ee_frame = '/l_gripper_shaver305_frame'
self.head_pose = None
self.tfl = TransformListener()
self.is_forced_retreat = False
self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array',
PoseArray)
self.global_input_sub = rospy.Subscriber(
"/face_adls/global_move", String, async_call(self.global_input_cb))
self.local_input_sub = rospy.Subscriber(
"/face_adls/local_move", String, async_call(self.local_input_cb))
self.clicked_input_sub = rospy.Subscriber(
"/face_adls/clicked_move", PoseStamped,
async_call(self.global_input_cb))
self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
self.global_move_poses_pub = rospy.Publisher(
'/face_adls/global_move_poses', StringArray, latch=True)
self.controller_enabled_pub = rospy.Publisher(
'/face_adls/controller_enabled', Bool, latch=True)
self.enable_controller_srv = rospy.Service(
"/face_adls/enable_controller", EnableFaceController,
self.enable_controller_cb)
self.request_registration = rospy.ServiceProxy("/request_registration",
RequestRegistration)
self.enable_mpc_srv = rospy.ServiceProxy('/haptic_mpc/enable_mpc',
EnableHapticMPC)
self.ell_params_pub = rospy.Publisher("/ellipsoid_params",
EllipsoidParams,
latch=True)
def stop_move_cb(msg):
self.stop_moving()
self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move",
Bool,
stop_move_cb,
queue_size=1)
def stop_moving(self):
"""Send empty PoseArray to skin controller to stop movement"""
for x in range(2):
self.pose_traj_pub.publish(
PoseArray()) #Send empty msg to skin controller
cart_traj_msg = [
PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))
]
head = Header()
head.frame_id = '/torso_lift_link'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
rospy.loginfo("Sent stop moving commands!")
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now,
rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame",
now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %
rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" %
(rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" % (rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %
(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(
Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 *
ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)
def current_ee_pose(self, link, frame='/torso_lift_link'):
"""Get current end effector pose from tf"""
# print "Getting pose of %s in frame: %s" %(link, frame)
try:
now = rospy.Time.now()
self.tfl.waitForTransform(frame, link, now, rospy.Duration(8.0))
pos, quat = self.tfl.lookupTransform(frame, link, now)
except (LookupException, ConnectivityException, ExtrapolationException,
Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure getting current end-effector pose: %s"
% e)
return None
return pos, quat
def publish_feedback(self, message=None):
if message is not None:
rospy.loginfo("[face_adls_manager] %s" % message)
self.feedback_pub.publish(message)
def enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side',
'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'],
bounds['height']) #TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id,
'/base_link', now,
rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose(
'/base_link', reg_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure converting ellipse to base frame: %s"
% e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(
self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l',
'%s_arm_controller',
reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
def setup_force_monitor(self):
self.force_monitor = ForceCollisionMonitor()
# registering force monitor callbacks
def dangerous_cb(force):
self.stop_moving()
curr_pose = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(curr_pose)
if ell_pos[2] < SAFETY_RETREAT_HEIGHT * 0.9:
self.publish_feedback(Messages.DANGEROUS_FORCE % force)
self.retreat_move(SAFETY_RETREAT_HEIGHT,
SAFETY_RETREAT_VELOCITY,
forced=True)
self.force_monitor.register_dangerous_cb(dangerous_cb)
def timeout_cb(time):
start_time = rospy.get_time()
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(
self.current_ee_pose(self.ee_frame, '/ellipse_frame'))
rospy.loginfo(
"ELL POS TIME: %.3f " % (rospy.get_time() - start_time) +
str(ell_pos) + " times: %.3f %.3f" %
(self.force_monitor.last_activity_time, rospy.get_time()))
if ell_pos[
2] < RETREAT_HEIGHT * 0.9 and self.force_monitor.is_inactive(
):
self.publish_feedback(Messages.TIMEOUT_RETREAT % time)
self.retreat_move(RETREAT_HEIGHT, LOCAL_VELOCITY)
self.force_monitor.register_timeout_cb(timeout_cb)
def contact_cb(force):
self.force_monitor.update_activity()
if not self.is_forced_retreat:
self.stop_moving()
self.publish_feedback(Messages.CONTACT_FORCE % force)
rospy.loginfo(
"[face_adls_manZger] Arm stopped due to making contact.")
self.force_monitor.register_contact_cb(contact_cb)
self.force_monitor.start_activity()
self.force_monitor.update_activity()
self.is_forced_retreat = False
def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame,
'/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0, 0, 0, 1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose(
(retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [
self.ellipsoid.ellipsoidal_to_pose(ell_pose)
for ell_pose in ell_path
]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START % msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id,
'/ellipse_frame', msg.header.stamp,
rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(
goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi / 2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(
goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg(
'/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback(
'Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(
goal_cart_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s"
% e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot,
[1., 0., 0., 0.])
approach_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT
]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
final_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1],
goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST
]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose(
(retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose(
(approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose(
(final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
retreat_ell_pos,
retreat_ell_quat,
velocity=0.01,
min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(
retreat_ell_pos,
retreat_ell_quat,
approach_ell_pos,
approach_ell_quat,
velocity=0.01,
min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(
approach_ell_pos,
approach_ell_quat,
final_ell_pos,
final_ell_quat,
velocity=0.01,
min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [
curr_ell_pos[0] + change_trans_ep[0],
curr_ell_pos[1] + change_trans_ep[1],
curr_ell_pos[2] + change_trans_ep[2]
]
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
goal_ell_pos,
curr_ell_quat,
velocity=ELL_LOCAL_VEL,
min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START %
button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos,
goal_ell_quat,
velocity=ELL_ROT_VEL,
min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
curr_ell_pos, (0., 0., 0., 1.),
velocity=ELL_ROT_VEL,
min_jerk=True)
else:
rospy.logerr(
"[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
self.force_monitor.update_activity()
class ArmActions():
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy('/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.safe_approach)
#rospy.Subscriber('norm_approach_left', PoseStamped, self.hfc_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg)
rospy.Subscriber('wt_hfc_swipe_left_goals', PoseStamped, self.hfc_swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point, self.force_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact)
rospy.Subscriber('wt_hfc_contact_approach_left', PoseStamped, self.hfc_approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
#rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('l_cart/ft_wrench', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_cont_switch', String, self.cont_switch)
#self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool)
self.rezero_wrench = rospy.Publisher('l_cart/ft_zero', Bool)
#rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
#rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
rospy.Subscriber('wt_ft_goal', Float32, self.hfc_set_force)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_mag_que = deque([0]*10,10)
self.ft_sm_mag = 0.
self.ft_case = None
self.force_goal_mean = 1.42
self.force_goal_std= 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
################### HFC ###################
rospy.Subscriber('wt_hfc_mannequin', Bool, self.hfc_mannequin)
self.active_cont = 'l_arm_controller'
self.cs = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
#kpt, kpr, kdt, kdr = 300, 8, 15, 4.2
#kfpt, kfpr, kfdt, kfdr = 3.8, 0.3, 0.1, 0.1 # force gains (derivative damps velocity)
#force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others
#self.arm.set_gains([kpt, kpt, kpt, kpr, kpr, kpr], [kdt, kdt, kdt, kdr, kdr, kdr], [kfpt, kfpt, kfpt, kfpr, kfpr, kfpr], [kfdt, kfdt, kfdt, kfdr, kfdr, kfdr], force_selector)
# force desired is currently at 0
self.hfc_pose_out = rospy.Publisher('/l_cart/command_pose', PoseStamped)
rospy.Subscriber('l_cart/state/x', PoseStamped, self.hfc_cur_pose)
def cont_switch(self, string_msg):
if string_msg.data == 'l_arm_controller':
self.switch_to_default()
elif string_msg.data == 'l_cart':
self.switch_to_hfc()
else:
rospy.logerr("Controller Name Not Recognized")
def switch_to_hfc(self):
self.cs.switch('l_arm_controller', 'l_cart')
self.active_cont = 'l_cart'
rospy.loginfo("Switching to Hybrid Force Controller")
def switch_to_default(self):
self.cs.switch('l_cart', 'l_arm_controller')
self.active_cont = 'l_arm_controller'
rospy.loginfo("Switching to Default Joint Angle Controller")
def hfc_cur_pose(self, ps):
self.hfc_cur_pose = ps
def hfc_set_force(self, force):
self.arm.zero_sensor()
if type(force) == type(Float32()):
self.send_hfc_pose(self.hfc_cur_pose)
force = force.data
self.arm.set_force_directions([1,0,0,0,0,0])
self.arm.update_gains()
self.arm.set_force([force, 0, 0, 0, 0, 0])
def send_hfc_pose(self, ps):
self.hfc_set_force(0.)
self.hfc_pose_out.publish(ps)
def hfc_slow_move(self, ps):
poses = self.aul.build_trajectory(ps,self.hfc_cur_pose)
pose_rate = rospy.Rate(4)
count = 0
self.hfc_set_force(0.)
self.arm.set_force_directions([0]*6) # Turn off force control
self.arm.update_gains()
while count < len(poses):
#print count, poses[count]
self.hfc_pose_out.publish(poses[count])
count += 1
pose_rate.sleep()
def hfc_approach(self, ps):
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps,0)
#self.send_hfc_pose(appr)
self.hfc_slow_move(appr)
def hfc_mannequin(self, token):
self.arm.set_force([0,0,0,0,0,0])
self.arm.set_force_directions([1,1,1,1,1,1])
self.arm.update_gains()
def hfc_approach_to_contact(self, ps):
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0.1)
self.hfc_slow_move(appr)
dist = self.aul.calc_dist(self.hfc_cur_pose, appr)
#prep = self.safe_approach(ps)
if True:#prep:
self.switch_to_hfc()
self.arm.zero_sensor()
self.arm.set_force([1.6,0,0,0,0,0])
self.arm.set_force_gains(p_trans=3.0)
self.arm.set_motion_gains(d_trans=1.0)
self.arm.set_force_directions([1,0,0,0,0,0])
self.arm.update_gains()
def hfc_swipe(self, ps):
self.hfc_approach_to_contact(ps)
while self.ft.wrench.force.x < 1:
rospy.sleep(0.1)
self.arm.set_force([1.0,0,3.0,0,0,0])
self.arm.set_force_directions([1,0,1,0,0,0])
self.arm.update_gains()
############ END HFC #################
def set_force_goal(self, msg):
self.force_goal_mean = msg.data
def ft_preprocess(self, ft):
self.ft = ft
self.ft_mag = math.sqrt(ft.wrench.force.x**2 + ft.wrench.force.y**2 + ft.wrench.force.z**2)
self.ft_mag_que.append(self.ft_mag)
self.ft_sm_mag = np.mean(self.ft_mag_que)
#print 'Force Magnitude: ', self.ft_mag
self.wt_force_out.publish(self.ft_mag)
def approach_to_contact(self, ps, overshoot=0.05):
ps.pose.position.z += 0.02
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0.15)
goal = self.aul.find_approach(ps, -overshoot)
(appr_reachable, ik_goal) = self.aul.full_ik_check(appr)
(goal_reachable, ik_goal) = self.aul.full_ik_check(goal)
if appr_reachable and goal_reachable:
traj = self.aul.build_trajectory(goal,appr,tot_points=1000)
#prep = self.aul.move_arm_to(appr)
self.aul.blind_move(appr)
if True: # if prep:
self.adjust_forearm(traj.points[0].positions)
self.rezero_wrench.publish(data=True)
curr_traj_point = self.advance_to_contact(traj)
if not curr_traj_point is None:
self.maintain_norm_force2(traj, curr_traj_point)
#self.maintain_force_position(self.aul.hand_frame_move(0.05))
#self.twist_wipe(); self.aul.blind_move(appr)
else:
self.aul.send_traj_point(traj.points[0], 4)
else:
rospy.loginfo("Cannot reach desired 'move-to-contact' point")
self.wt_log_out.publish(data="Cannot reach desired 'move-to-contact' point")
def advance_to_contact(self, traj):
self.stop_maintain = False
curr_traj_point = 0
advance_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point], 0.01)
curr_traj_point += 1
advance_rate.sleep()
else:
rospy.loginfo("Moved past expected contact, but no contact found! Returning to start")
self.wt_log_out.publish(data="Moved past expected contact, but no contact found! Returning to start")
self.stop_maintain = True
return None
if self.ft_mag > 1.5:
self.stop_maintain = True
print "Contact Detected"
return curr_traj_point
def maintain_norm_force2(self, traj, curr_traj_point=0, mean=0, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(1000)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft_mag > 12:
rospy.loginfo("Force Too High, ending behavior")
self.wt_log_out.publish(data="Force too high, ending behavior")
break
#print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag
if self.ft_mag < mean - std:
curr_traj_point += 1
curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points))
#print curr_traj_point
print "Low"
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point], 0.01)
#else:
# rospy.loginfo("Force too low, but extent of the trajectory is reached")
# self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached")
# self.stop_maintain = True
elif self.ft_mag > mean + std:
print "High"
steps = int(round((self.ft_mag/std)))
curr_traj_point -= steps
#print curr_traj_point
curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points))
if curr_traj_point >= 0:
self.aul.send_traj_point(traj.points[curr_traj_point], 0.009)
else:
rospy.loginfo("Beginning of trajectory reached, cannot back away further")
self.wt_log_out.publish(data="Beginning of trajectory reached, cannot back away further")
#self.stop_maintain = True
maintain_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
print "Returning to start position"
self.aul.send_traj_point(traj.points[0], 4)
def maintain_norm_force(self, traj, curr_traj_point=0, mean=0, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(1000)
while not (rospy.is_shutdown() or self.stop_maintain):
#print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag
if self.ft_mag < mean - std:
curr_traj_point += 1
np.clip(curr_traj_point, 0, len(traj.points))
if not (curr_traj_point >= len(traj.points)):
print curr_traj_point
self.aul.send_traj_point(traj.points[curr_traj_point], 0.0025)
#rospy.sleep(0.002)
else:
rospy.loginfo("Force too low, but extent of the trajectory is reached")
self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached")
stopped = True
elif self.ft_mag > mean + std:
curr_traj_point -= 1
np.clip(curr_traj_point, 0, len(traj.points))
if curr_traj_point >= 0:
print curr_traj_point
self.aul.send_traj_point(traj.points[curr_traj_point], 0.0001)
#rospy.sleep(0.002)
else:
rospy.loginfo("Beginning of trajectory reached, cannot back away further")
self.wt_log_out.publish(data="Beginning of trajectory reached, cannot back away further")
stopped = True
maintain_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
# def maintain_net_force(self, mean=0, std=3):
# self.stop_maintain = False
# maintain_rate = rospy.Rate(100)
# while not (rospy.is_shutdown() or self.stop_maintain):
# if self.ft_mag > mean + 8:
# curr_angs = self.aul.joint_state_act.positions
# try:
# x_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0.02))).solution.joint_state.position, curr_angs)
# x_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(-0.02))).solution.joint_state.position, curr_angs)
# y_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0.02, 0))).solution.joint_state.position, curr_angs)
# y_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, -0.02, 0))).solution.joint_state.position, curr_angs)
# z_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, 0.02))).solution.joint_state.position, curr_angs)
# z_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, -0.02))).solution.joint_state.position, curr_angs)
# #print 'x: ', x_plus,'\r\n', x_minus
# #print 'y: ', y_plus,'\r\n', y_minus
# #print 'z: ', z_plus,'\r\n', z_minus
# ft_sum = self.ft_mag
# parts = np.divide([self.ft.wrench.force.x, self.ft.wrench.force.y, self.ft.wrench.force.z], ft_sum)
# print 'parts', parts
# ends = [[x_plus,x_minus],[y_plus, y_minus],[z_plus,z_minus]]
# side = [[0]*7]*3
# for i, part in enumerate(parts):
# if part >=0:
# side[i] = ends[i][0]
# else:
# side[i] = ends[i][1]
#
# ang_out = curr_angs
# for i in range(3):
# ang_out -= np.average(side, 0, parts)
# except:
# print 'Near Joint Limits!'
# self.aul.send_joint_angles(ang_out)
#
# #print 'x: ', x_plus, x_minus
# maintain_rate.sleep()
# mean = self.force_goal_mean
# std = self.force_goal_std
def maintain_force_position(self,pose = None, mean=3, std=1):
self.stop_maintain = False
if pose is None:
goal = self.aul.curr_pose()
else:
goal = pose
goal_ort = [goal.pose.orientation.x,goal.pose.orientation.y,goal.pose.orientation.z,goal.pose.orientation.w]
error = PoseStamped()
maintain_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
current = self.aul.curr_pose()
current_ort = [current.pose.orientation.x, current.pose.orientation.y, current.pose.orientation.z, current.pose.orientation.w]
error.pose.position.x = current.pose.position.x - goal.pose.position.x
error.pose.position.y = current.pose.position.y - goal.pose.position.y
error.pose.position.z = current.pose.position.z - goal.pose.position.z
error_mag = math.sqrt(error.pose.position.x**2 + error.pose.position.y**2 + error.pose.position.z**2)
out = deepcopy(goal)
out.header.stamp = rospy.Time.now()
if all(np.array(self.ft_mag_que) < mean - std) and error_mag > 0.005:
print 'Force Too LOW'
out.pose.position.x += 0.990*error.pose.position.x
out.pose.position.y += 0.990*error.pose.position.y
out.pose.position.z += 0.990*error.pose.position.z
ori = transformations.quaternion_slerp(goal_ort, current_ort, 0.990)
out.pose.orientation = Quaternion(*ori)
self.aul.fast_move(out, max(0.3*error_mag, 0.05))
self.test_pose.publish(out)
elif all(np.array(self.ft_mag_que) > mean + std):
print 'Moving to avoid force'
current.pose.position.x += self.ft.wrench.force.x/9000
current.pose.position.y += self.ft.wrench.force.y/9000
current.pose.position.z += self.ft.wrench.force.z/9000
self.aul.fast_move(current, 0.02)
self.test_pose.publish(current)
else:
print "Force in desired range"
mean = self.force_goal_mean
std = self.force_goal_std
rospy.sleep(0.001)
maintain_rate.sleep()
#def maintain_force_position(self,pose = None, mean=3, std=1):
# self.stop_maintain = False
# if pose is None:
# goal = self.aul.curr_pose()
# else:
# goal = pose
# self.rezero_wrench.publish(data=True)
# maintain_rate = rospy.Rate(500)
# kp = 0.07
# kd = 0.03
# ki = 0.0
# error = PoseStamped()
# old_error = PoseStamped()
# sum_error_x = deque([0]*10,10)
# sum_error_y = deque([0]*10,10)
# sum_error_z = deque([0]*10,10)
# while not (rospy.is_shutdown() or self.stop_maintain):
# current = self.aul.curr_pose()
# error.pose.position.x = current.pose.position.x - goal.pose.position.x
# error.pose.position.y = current.pose.position.y - goal.pose.position.y
# error.pose.position.z = current.pose.position.z - goal.pose.position.z
# sum_error_x.append(error.pose.position.x)
# sum_error_y.append(error.pose.position.y)
# sum_error_z.append(error.pose.position.z)
# print "Force: ", self.ft_sm_mag, min(self.ft_mag_que), max(self.ft_mag_que)
# print "Error: ", error.pose.position
# print self.ft_mag_que, mean-std
# print self.ft_mag_que < mean-std
# break
# if all([self.ft_mag_que < mean - std]):
# print 'Force Too LOW'
# current.pose.position.x += kp*error.pose.position.x + kd*(error.pose.position.x - old_error.pose.position.x) + ki*np.sum(sum_error_x)
# current.pose.position.x += kp*error.pose.position.y + kd*(error.pose.position.y - old_error.pose.position.y) + ki*np.sum(sum_error_y)
# current.pose.position.x += kp*error.pose.position.z + kd*(error.pose.position.z - old_error.pose.position.z) + ki*np.sum(sum_error_z)
# self.aul.fast_move(current, 0.02)
# self.test_pose.publish(current)
# if all([i > mean + std for i in self.ft_mag_que]):
# print 'Moving to avoid force'
# current.pose.position.x += self.ft.wrench.force.x/10000
# current.pose.position.y += self.ft.wrench.force.y/10000
# current.pose.position.z += self.ft.wrench.force.z/10000
# self.aul.fast_move(current, 0.02)
# self.test_pose.publish(current)
# old_error = error
# mean = self.force_goal_mean
# std = self.force_goal_std
# maintain_rate.sleep()
def maintain_net_force(self, mean=3, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(500)
self.rezero_wrench.publish(data=True)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft_mag > mean + 3:
print 'Moving to avoid force'
print "Force: ", self.ft_mag
goal = self.aul.curr_pose()
goal.pose.position.x += np.clip(self.ft.wrench.force.x/5000, -0.001, 0.001)
goal.pose.position.y += np.clip(self.ft.wrench.force.y/5000, -0.001, 0.001)
goal.pose.position.z += np.clip(self.ft.wrench.force.z/5000, -0.001, 0.001)
self.test_pose.publish(goal)
self.aul.fast_move(goal, 0.02)
mean = self.force_goal_mean
std = self.force_goal_std
maintain_rate.sleep()
def mannequin(self):
mannequin_rate=rospy.Rate(100)
pose = PoseStamped()
while not rospy.is_shutdown():
#joints = np.add(np.array(self.aul.joint_state_act.positions), np.clip(np.array(self.aul.joint_state_err.positions), -0.05, 0.05))
joints = self.aul.joint_state_act.positions
print joints
#raw_input('Review Joint Angles')
self.aul.send_joint_angles(joints, 0.00001)
pose.header.stamp = rospy.Time.now()
self.test_pose.publish(pose)
mannequin_rate.sleep()
def force_wipe_agg(self, ps):
ps.pose.position.z += 0.02
self.aul.update_frame(ps)
rospy.sleep(0.1)
pose = self.aul.find_approach(ps, 0)
(goal_reachable, ik_goal) = self.aul.ik_check(pose)
if goal_reachable:
if not self.force_wipe_started:
appr = self.aul.find_approach(ps, 0.20)
(appr_reachable, ik_goal) = self.aul.ik_check(appr)
self.test_pose.publish(appr)
if appr_reachable:
self.force_wipe_start = pose
self.force_wipe_appr = appr
self.force_wipe_started = True
else:
rospy.loginfo("Cannot reach approach point, please choose another")
self.wt_log_out.publish(data="Cannot reach approach point, please choose another")
self.say.publish(data="I cannot get to a safe approach for there, please choose another point")
else:
ps1, ps2 = self.align_poses(self.force_wipe_start, pose)
self.force_wipe_prep(ps1, ps2)
# self.force_wipe_prep(self.force_wipe_start, pose)
self.force_wipe_started = False
else:
rospy.loginfo("Cannot reach wiping point, please choose another")
self.wt_log_out.publish(data="Cannot reach wiping point, please choose another")
self.say.publish(data="I cannot reach there, please choose another point")
def force_wipe_prep(self, ps_start, ps_finish, travel = 0.05):
ps_start.header.stamp = rospy.Time.now()
ps_finish.header.stamp = rospy.Time.now()
ps_start_far = self.aul.hand_frame_move(travel, 0, 0, ps_start)
ps_start_near = self.aul.hand_frame_move(-travel, 0, 0, ps_start)
ps_finish_far = self.aul.hand_frame_move(travel, 0, 0, ps_finish)
ps_finish_near = self.aul.hand_frame_move(-travel, 0, 0, ps_finish)
n_points = int(math.ceil(self.aul.calc_dist(ps_start, ps_finish)*9000))
print 'n_points: ', n_points
mid_traj = self.aul.build_trajectory(ps_finish, ps_start, tot_points=n_points, jagged=False)
near_traj = self.aul.build_trajectory(ps_finish_near, ps_start_near, tot_points=n_points, jagged=False)
far_traj = self.aul.build_trajectory(ps_finish_far, ps_start_far, tot_points=n_points, jagged=False)
self.force_wipe(mid_traj, near_traj, far_traj)
def force_surf_wipe(self, point):
self.fsw_poses = self.prep_surf_wipe(point)
if not self.fsw_poses is None:
near_poses = far_poses = [PoseStamped() for i in xrange(len(self.fsw_poses))]
for i, p in enumerate(self.fsw_poses):
near_poses[i]=self.aul.pose_frame_move(p, -0.05)
far_poses[i]=self.aul.pose_frame_move(p, 0.05)
near_traj = self.aul.fill_ik_traj(near_poses)
mid_traj = self.aul.fill_ik_traj(self.fsw_poses)
far_traj = self.aul.fill_ik_traj(far_poses)
print 'Trajectories Found'
self.force_wipe(mid_traj, near_traj, far_traj)
def adjust_forearm(self, in_angles):
print 'cur angles: ', self.aul.joint_state_act.positions, 'angs: ', in_angles
print np.abs(np.subtract(self.aul.joint_state_act.positions, in_angles))
if np.max(np.abs(np.subtract(self.aul.joint_state_act.positions,in_angles)))>math.pi:
self.say.publish(data="Adjusting for-arm roll")
print "Evasive Action!"
angles = list(self.aul.joint_state_act.positions)
flex = in_angles[5]
angles[5] = -0.1
self.aul.send_joint_angles(angles, 4)
angles[4] = in_angles[4]
self.aul.send_joint_angles(angles,6)
angles[5] = flex
self.aul.send_joint_angles(angles, 4)
def force_wipe(self, mid_traj, near_traj, far_traj):
near_angles = [list(near_traj.points[i].positions) for i in range(len(near_traj.points))]
mid_angles = [list(mid_traj.points[i].positions) for i in range(len(mid_traj.points))]
far_angles = [list(far_traj.points[i].positions) for i in range(len(far_traj.points))]
print 'lens: nmf: ', len(near_angles), len(mid_angles), len(far_angles)
fmn_diff = np.abs(np.subtract(near_angles, far_angles))
fmn_max = np.max(fmn_diff, axis=0)
print 'fmn_max: ', fmn_max
if any(fmn_max >math.pi/2):
rospy.loginfo("TOO LARGE OF AN ANGLE CHANGE ALONG GRADIENT, IK REGION PROBLEMS!")
self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)")
self.say.publish(data="Large motion detected, cancelling. Please try again.")
return
for i in range(7):
n_max = max(np.abs(np.diff(near_angles,axis=0)[i]))
m_max = max(np.abs(np.diff(mid_angles,axis=0)[i]))
f_max = max(np.abs(np.diff(far_angles,axis=0)[i]))
n_mean = 4*np.mean(np.abs(np.diff(near_angles,axis=0)[i]))
m_mean = 4*np.mean(np.abs(np.diff(mid_angles,axis=0)[i]))
f_mean = 4*np.mean(np.abs(np.diff(far_angles,axis=0)[i]))
print 'near: max: ', n_max, 'mean: ', n_mean
print 'mid: max: ', m_max, 'mean: ', m_mean
print 'far: max: ', f_max, 'mean: ', f_mean
if (n_max >= n_mean) or (m_max >= m_mean) or (f_max >= f_mean):
rospy.logerr("TOO LARGE OF AN ANGLE CHANGE ALONG PATH, IK REGION PROBLEMS!")
self.wt_log_out.publish(data="The path requested required large movements (IK Limits cause angle wrapping)")
self.say.publish(data="Large motion detected, cancelling. Please try again.")
return
near_diff = np.subtract(near_angles, mid_angles).tolist()
far_diff = np.subtract(far_angles, mid_angles).tolist()
self.say.publish(data="Moving to approach point")
appr = self.force_wipe_appr
appr.pose.orientation = self.aul.get_fk(near_angles[0]).pose.orientation
#prep = self.aul.move_arm_to(appr)
self.aul.blind_move(appr)
#rospy.sleep(3)
if True: #prep:
self.adjust_forearm(near_angles[0])
self.say.publish(data="Making Approach.")
bias = 2
self.aul.send_joint_angles(np.add(mid_angles[0],np.multiply(bias, near_diff[0])), 3.5)
self.rezero_wrench.publish(data=True)
rospy.sleep(1)
wipe_rate = rospy.Rate(1000)
self.stop_maintain = False
count = 0
lap = 0
max_laps = 4
mean = self.force_goal_mean
std = self.force_goal_std
self.say.publish(data="Wiping")
single_dir = False#True
time = rospy.Time.now().to_sec()
while not (rospy.is_shutdown() or self.stop_maintain) and (count + 1 <= len(mid_angles)) and (lap < max_laps):
if self.ft_mag > 10:
angles_out = np.add(mid_angles[0], np.multiply(2, near_diff[0]))
self.aul.send_joint_angles(angles_out,2)
rospy.loginfo("Force Too High, ending behavior")
self.wt_log_out.publish(data="Force too high, ending behavior")
break
#print "mean: ", mean, "std: ", std, "force: ", self.ft_mag
if self.ft_mag >= mean + std:
# print 'Force too high!'
bias += (self.ft_mag/500)
elif self.ft_mag < mean - std:
# print 'Force too low'
bias -= max(0.003,(self.ft_mag/1500))
else:
# print 'Force Within Range'
count += 1
bias = np.clip(bias, -1, 2)
if bias > 0.:
diff = near_diff[count]
else:
diff = far_diff[count]
angles_out = np.add(mid_angles[count], np.multiply(abs(bias), diff))
self.aul.send_joint_angles(angles_out, 0.0025)
#rospy.sleep(0.0000i1)
#print "Rate: ", (1/ (rospy.Time.now().to_sec() - time))
#time = rospy.Time.now().to_sec()
wipe_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
if count + 1>= len(mid_angles):
if single_dir:
bias = 1
pose = self.aul.curr_pose()
pose = self.aul.hand_frame_move(-0.01)
rot = transformations.quaternion_about_axis(math.radians(-10), (0,1,0))
q = transformations.quaternion_multiply([pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w],rot)
pose.pose.orientation = Quaternion(*q)
self.aul.blind_move(pose)
#goal = self.aul.ik_pose_proxy(self.aul.form_ik_request(pose))
#if goal.error_code.val == 1:
# self.aul.send_angles_wrap(goal.solution.joint_state.position)
#angles_out = list(self.aul.joint_state_act.positions)
#angles_out[4] += 0.05
# self.aul.send_joint_angles(angles_out,3)
angles_out = np.add(mid_angles[count], np.multiply(bias, near_diff[count]))
self.aul.send_joint_angles(angles_out,5)
angles_out = np.add(mid_angles[0], np.multiply(bias, near_diff[0]))
self.aul.send_joint_angles(angles_out,5)
else:
mid_angles.reverse()
near_diff.reverse()
far_diff.reverse()
lap += 1
#if lap == 3:
# self.say.publish(data="Hold Still, you rascal!")
count = 0
rospy.sleep(0.5)
self.say.publish(data="Pulling away")
angles_out = np.add(mid_angles[0], np.multiply(2, near_diff[0]))
self.aul.send_joint_angles(angles_out,5)
rospy.sleep(5)
self.say.publish(data="Finished wiping. Thank you")
def torso_frame_move(self, msg):
self.stop_maintain = True
goal = self.aul.curr_pose()
goal.pose.position.x += msg.x
goal.pose.position.y += msg.y
goal.pose.position.z += msg.z
self.aul.blind_move(goal)
def hand_move(self, f32):
self.stop_maintain = True
hfm_pose = self.aul.hand_frame_move(f32.data)
self.aul.blind_move(hfm_pose)
def safe_approach(self, ps, standoff = 0.15):
self.stop_maintain = True
print 'Controller: ', self.active_cont
if self.active_cont != 'l_arm_controller':
self.switch_to_default()
rospy.sleep(1)
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, standoff)
arrived = self.aul.move_arm_to(appr)
return arrived
def grasp(self, ps):
self.stop_maintain = True
rospy.loginfo("Initiating Grasp Sequence")
self.wt_log_out.publish(data="Initiating Grasp Sequence")
self.aul.update_frame(ps)
approach = self.aul.find_approach(ps, 0.15)
rospy.loginfo("approach: \r\n %s" %approach)
at_appr = self.aul.move_arm_to(approach)
rospy.loginfo("arrived at approach: %s" %at_appr)
if at_appr:
opened = self.aul.gripper(0.09)
if opened:
rospy.loginfo("making linear approach")
#hfm_pose = self.aul.hand_frame_move(0.23)
hfm_pose = self.aul.find_approach(ps,-0.02)
self.aul.blind_move(hfm_pose)
self.aul.wait_for_stop(2)
closed = self.aul.gripper(0.0)
if not closed:
rospy.loginfo("Couldn't close completely: Grasp likely successful")
hfm_pose = self.aul.hand_frame_move(-0.23)
self.aul.blind_move(hfm_pose)
else:
pass
def prep_surf_wipe(self, point):
pixel_u = point.x
pixel_v = point.y
test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d
self.aul.update_frame(test_pose)
test_pose = self.aul.find_approach(test_pose, 0)
(reachable, ik_goal) = self.aul.full_ik_check(test_pose)
if reachable:
if not self.surf_wipe_started:
start_pose = test_pose
self.surf_wipe_start = [pixel_u, pixel_v, start_pose]
self.surf_wipe_started = True
rospy.loginfo("Received valid starting position for wiping action")
self.wt_log_out.publish(data="Received valid starting position for wiping action")
return None#Return after 1st point, wait for second
else:
rospy.loginfo("Received valid ending position for wiping action")
self.wt_log_out.publish(data="Received valid ending position for wiping action")
self.surf_wipe_started = False #Continue on successful 2nd point
else:
rospy.loginfo("Cannot reach wipe position, please try another")
self.wt_log_out.publish(data="Cannot reach wipe position, please try another")
return None#Return on invalid point, wait for another
dist = self.aul.calc_dist(self.surf_wipe_start[2],test_pose)
print 'dist', dist
num_points = dist/0.01
print 'num_points', num_points
us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u, num_points))
vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v, num_points))
surf_points = [PoseStamped() for i in xrange(len(us))]
print "Surface Points", [us,vs]
for i in xrange(len(us)):
pose = self.p23d_proxy(us[i],vs[i]).pixel3d
self.aul.update_frame(pose)
surf_points[i] = self.aul.find_approach(pose,0)
print i+1, '/', len(us)
return surf_points
#self.aul.blind_move(surf_points[0])
#self.aul.wait_for_stop()
#for pose in surf_points:
# self.aul.blind_move(pose,2.5)
# rospy.sleep(2)
# #self.aul.wait_for_stop()
#self.aul.hand_frame_move(-0.1)
def twist_wipe(self):
angles = list(self.aul.joint_state_act.positions)
count = 0
while count < 3:
angles[6] = -6.7
self.aul.send_joint_angles(angles)
while self.aul.joint_state_act.positions[6] > -6.6:
rospy.sleep(0.1)
angles[6] = 0.8
self.aul.send_joint_angles(angles)
while self.aul.joint_state_act.positions[6] < 0.7:
rospy.sleep(0.1)
count += 1
def poke(self, ps):
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps,0.15)
touch = self.aul.find_approach(ps,0)
prepared = self.aul.move_arm_to(appr)
if prepared:
self.aul.blind_move(touch)
self.aul.wait_for_stop()
rospy.sleep(7)
self.aul.blind_move(appr)
def swipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 1)
def wipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 4)
def prep_wipe(self, ps):
#print "Prep Wipe Received: %s" %pa
self.aul.update_frame(ps)
print "Updating frame to: %s \r\n" %ps
if not self.wipe_started:
self.wipe_appr_seed = ps
self.wipe_ends[0] = self.aul.find_approach(ps, 0)
print "wipe_end[0]: %s" %self.wipe_ends[0]
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[0])
if not reachable:
rospy.loginfo("Cannot find approach for initial wipe position, please try another")
self.wt_log_out.publish(data="Cannot find approach for initial wipe position, please try another")
return None
else:
self.wipe_started = True
rospy.loginfo("Received starting position for wiping action")
self.wt_log_out.publish(data="Received starting position for wiping action")
return None
else:
self.wipe_ends[1] = self.aul.find_approach(ps, 0)
self.wipe_ends.reverse()
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[1])
if not reachable:
rospy.loginfo("Cannot find approach for final wipe position, please try another")
self.wt_log_out.publish(data="Cannot find approach for final wipe position, please try another")
return None
else:
rospy.loginfo("Received End position for wiping action")
self.wt_log_out.publish(data="Received End position for wiping action")
####### REMOVED AND REPLACED WITH ALIGN FUNCTION ##############
self.wipe_ends[0], self.wipe_ends[1] = self.align_poses(self.wipe_ends[0],self.wipe_ends[1])
self.aul.update_frame(self.wipe_appr_seed)
appr = self.aul.find_approach(self.wipe_appr_seed, 0.15)
appr.pose.orientation = self.wipe_ends[1].pose.orientation
prepared = self.aul.move_arm_to(appr)
if prepared:
#self.aul.advance_to_contact()
self.aul.blind_move(self.wipe_ends[1])
traj = self.aul.build_trajectory(self.wipe_ends[1], self.wipe_ends[0])
wipe_traj = self.aul.build_follow_trajectory(traj)
self.aul.wait_for_stop()
self.wipe_started = False
return wipe_traj
#self.wipe(wipe_traj)
else:
rospy.loginfo("Failure reaching start point, please try again")
self.wt_log_out.publish(data="Failure reaching start point, please try again")
def align_poses(self, ps1, ps2):
self.aul.update_frame(ps1)
ps2.header.stamp = rospy.Time.now()
self.tfl.waitForTransform(ps2.header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0))
ps2_in_ps1 = self.tfl.transformPose('lh_utility_frame', ps2)
ang = math.atan2(-ps2_in_ps1.pose.position.z, -ps2_in_ps1.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply([ps1.pose.orientation.x, ps1.pose.orientation.y, ps1.pose.orientation.z, ps1.pose.orientation.w],q_st_rot)
ps1.pose.orientation = Quaternion(*q_st_new)
self.aul.update_frame(ps2)
ps1.header.stamp = rospy.Time.now()
self.tfl.waitForTransform(ps1.header.frame_id, 'lh_utility_frame', rospy.Time.now(), rospy.Duration(3.0))
ps1_in_ps2 = self.tfl.transformPose('lh_utility_frame', ps1)
ang = math.atan2(ps1_in_ps2.pose.position.z, ps1_in_ps2.pose.position.y)+(math.pi/2)
q_st_rot = transformations.quaternion_about_axis(ang, (1,0,0))
q_st_new = transformations.quaternion_multiply([ps2.pose.orientation.x, ps2.pose.orientation.y, ps2.pose.orientation.z, ps2.pose.orientation.w],q_st_rot)
ps2.pose.orientation = Quaternion(*q_st_new)
return ps1, ps2
def wipe_move(self, traj_goal, passes=4):
times = []
for i in range(len(traj_goal.trajectory.points)):
times.append(traj_goal.trajectory.points[i].time_from_start)
count = 0
while count < passes:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
traj_goal.trajectory.header.stamp = rospy.Time.now()
assert traj_goal.trajectory.points[0] != []
self.aul.l_arm_follow_traj_client.send_goal(traj_goal)
self.aul.l_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)# Pause at end of swipe
count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
hfm_pose = self.aul.hand_frame_move(-0.15)
self.aul.blind_move(hfm_pose)
class Shaving_manager():
def __init__(self):
self.tf = TransformListener()
self.jtarms = jttask_utils.jt_task_utils(self.tf)
self.controller_switcher = ControllerSwitcher()
##### Subscribers ####
self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8, self.disc_change_pose)
self.disc_sub.impl.add_callback(self.cancel_goals, None)
self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point, self.cont_change_pose)
self.cont_sub.impl.add_callback(self.cancel_goals, None)
self.loc_sub = rospy.Subscriber('wt_shave_location', Int8, self.set_shave_pose)
self.loc_sub.impl.add_callback(self.cancel_goals, None)
rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving)
#### Services ####
rospy.loginfo("Waiting for get_head_pose service")
try:
rospy.wait_for_service('/get_head_pose', 5.0)
self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose', GetHeadPose)
rospy.loginfo("Found get_head_pose service")
except:
rospy.logwarn("get_head_pose service not available")
rospy.loginfo("Waiting for ellipsoid_command service")
try:
rospy.wait_for_service('/ellipsoid_command', 5.0)
self.ellipsoid_command_proxy = rospy.ServiceProxy('/ellipsoid_command', EllipsoidCommand)
rospy.loginfo("Found ellipsoid_command service")
except:
rospy.logwarn("ellipsoid_command service not available")
#### Publishers ####
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_out = rospy.Publisher('test_pose_1', PoseStamped)
self.rezero_wrench = rospy.Publisher('/netft_gravity_zeroing/rezero_wrench', Bool)
#### Data ####
self.hand_offset = 0.195
self.angle_tool_offset = 0.03
self.inline_tool_offset = 0.085
self.selected_pose = 0
self.poses = {
0 : ["near_ear",0.],
1 : ["upper_cheek",0.],
2 : ["middle_cheek",0.],
3 : ["jaw_bone",0.],
4 : ["back_neck",0.],
5 : ["nose",0.],
6 : ["chin",0.],
7 : ["mouth_corner", 0.]
}
#self.ft_wrench = WrenchStamped()
# self.force_unsafe = False
# self.ft_activity_thresh = 3
# self.ft_safety_thresh = 12
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
# rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state)
if POSTURE:
rospy.sleep(3)
print 'Sending Posture'
self.jtarms.send_posture(posture='elbowup', arm=0)
# def get_ft_state(self, ws):
# self.ft_wrench = ws
# self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
# if self.ft_mag > self.ft_safety_thresh:
# self.force_unsafe = True
# if self.ft_mag > self.ft_activity_thresh:
# self.ft_activity = rospy.Time.now()
def stop_shaving(self, msg):
self.cancel_goals(msg)
rospy.loginfo("Stopping Shaving Behavior")
self.wt_log_out.publish(data="Stopping Shaving Behavior")
self.controller_switcher.carefree_switch('l','%s_arm_controller')
def cancel_goals(self, msg):
self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
rospy.loginfo("Cancelling Active Shaving Goals")
self.wt_log_out.publish(data="Cancelling Active Shaving Goals")
def cont_change_pose(self, step):
self.controller_switcher.carefree_switch('l','%s_cart')
self.new_pose = True
req = EllipsoidCommandRequest()
req.change_latitude = int(-step.y)
req.change_longitude = int(-step.x)
req.change_height = int(step.z)
print req
print type(req.change_latitude)
self.ellipsoid_command_proxy.call(req)
rospy.loginfo("Moving Across Ellipsoid")
self.wt_log_out.publish("Moving Across Ellipsoid")
def disc_change_pose(self, step):
self.new_pose = True
self.selected_pose += step.data
if self.selected_pose > 7:
self.selected_pose = 7
self.wt_log_out.publish(data="Final position in sequence, there is no next position")
return
elif self.selected_pose < 0:
self.selected_pose = 0
self.wt_log_out.publish(data="First position in sequence, there is no previous position")
return
rospy.loginfo("Moving to "+self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def set_shave_pose(self, num):
self.selected_pose = num.data
rospy.loginfo("Moving to "+self.poses[self.selected_pose][0])
self.wt_log_out.publish(data="Moving to "+self.poses[self.selected_pose][0])
ellipse_pose = self.get_pose_proxy(self.poses[self.selected_pose][0], self.poses[self.selected_pose][1]).pose
self.adjust_pose(ellipse_pose)
def adjust_pose(self, ellipse_pose):
self.controller_switcher.carefree_switch('l','%s_cart')
self.jtarms.ft_move_client.cancel_all_goals() #Stop what we're doing, as we're about to do something different, and stopping soon may be desired
self.jtarms.ft_hold_client.cancel_all_goals()
#self.test_out.publish(ellipse_pose)
print "New Position Received, should stop current action"
self.tf.waitForTransform(ellipse_pose.header.frame_id, 'torso_lift_link', ellipse_pose.header.stamp, rospy.Duration(3.0))
torso_pose = self.tf.transformPose('torso_lift_link', ellipse_pose)
if TOOL == 'inline':
goal_pose = self.jtarms.pose_frame_move(torso_pose, -(self.hand_offset + self.inline_tool_offset), arm=0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
elif TOOL == '90':
goal_pose = self.jtarms.pose_frame_move(torso_pose, 0.015)
elif TOOL == '45':
goal_pose = torso_pose
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0)
self.ft_move(traj_to_approach)
self.rezero_wrench.publish(data=True)
self.ft_move(traj_appr_to_goal, ignore_ft=False)
retreat_traj = self.jtarms.build_trajectory(approach_pose, arm=0)
escape_traj = self.jtarms.build_trajectory(approach_pose, arm=0, steps=30)
self.jtarms.ft_hold_client.send_goal(FtHoldGoal(2.,8,10,rospy.Duration(10)))#activity_thresh, z_unsafe, mag_unsafe, timeout
print "Waiting for hold result"
finished = self.jtarms.ft_hold_client.wait_for_result(rospy.Duration(0))
print "Finished before Timeout: %s" %finished
print "Done Waiting"
hold_result = self.jtarms.ft_hold_client.get_result()
print "Hold Result:"
print hold_result
if hold_result.unsafe:
self.ft_move(escape_traj)
elif hold_result.timeout:
self.ft_move(retreat_traj)
def ft_move(self, traj, ft_thresh=2, ignore_ft=True):
self.jtarms.ft_move_client.send_goal(FtMoveGoal(traj, ft_thresh, ignore_ft),feedback_cb=self.ft_move_feedback)
finished_within_timeout = self.jtarms.ft_move_client.wait_for_result(rospy.Duration(0.25*len(traj)))
if finished_within_timeout:
result = self.jtarms.ft_move_client.get_result()
if result.all_sent:
rospy.loginfo("Full Trajectory Completed")
self.wt_log_out.publish(data="Full Trajectory Completed")
elif result.contact:
rospy.loginfo("Stopped Trajectory upon contact")
self.wt_log_out.publish(data="Stopped Trajectory upon contact")
else:
self.jtarms.ft_move_client.cancel_goal()
rospy.loginfo("Failed to complete movement within timeout period.")
self.wt_log_out.publish(data="Failed to complete movement within timeout period.")
def ft_move_feedback(self, fdbk):
pct = 100.*float(fdbk.current)/float(fdbk.total)
#rospy.loginfo("Movement is %2.0f percent complete." %pct)
self.wt_log_out.publish(data="Movement to %s is %2.0f percent complete."
%(self.poses[self.selected_pose][0], pct))
#def hold_ft_aware(self, escape_pose, arm=0):
# self.jtarms.force_stopped = False
# self.force_unsafe = False
# escape_traj = self.jtarms.build_trajectory(escape_pose)
# time_since_activity = rospy.Duration(0)
# self.ft_activity = rospy.Time.now()
# while not (rospy.is_shutdown() or self.force_unsafe or time_since_activity>rospy.Duration(10) or self.new_pose):
# time_since_activity = rospy.Time.now()-self.ft_activity
# rospy.sleep(0.01)
# if self.force_unsafe:
# rospy.loginfo("Unsafe High Forces, moving away!")
# self.wt_log_out.publish(data="Unsafe High Forces, moving away!")
# self.jtarms.goal_pub[arm].publish(escape_pose)
# return
# if time_since_activity>rospy.Duration(10):
# rospy.loginfo("10s of inactivity, moving away")
# self.wt_log_out.publish(data="10s of inactivity, moving away")
# if self.new_pose:
# rospy.loginfo("New Pose Received")
# self.wt_log_out.publish(data="New Pose Received")
# self.new_pose = False
# self.jtarms.send_traj(escape_traj)
def test(self):
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'torso_lift_link'
goal_pose.header.stamp = rospy.Time.now()
goal_pose.pose.position = Point(0.8, 0.3, 0.0)
goal_pose.pose.orientation = Quaternion(1,0,0,0)
approach_pose = self.jtarms.pose_frame_move(goal_pose, -0.15, arm=0)
traj_to_approach = self.jtarms.build_trajectory(approach_pose, arm=0)
traj_appr_to_goal = self.jtarms.build_trajectory(goal_pose, approach_pose, arm=0)
raw_input("Move to approach pose")
self.jtarms.send_traj(traj_to_approach)
self.jtarms.force_stopped = False
self.force_unsafe = False
raw_input("Move to contact pose")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.jtarms.send_traj_to_contact(traj_appr_to_goal)
raw_input("Hold under force constraints")
self.jtarms.force_stopped = False
self.force_unsafe = False
self.hold_ft_aware(approach_pose, arm=0)
rospy.loginfo("Finished Testing")
class CartesianControllerManager(object):
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber("/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber("/face_adls/%s_cart_absolute" % arm_char,
PoseStamped, async_call(self.command_absolute_cb))
self.adjust_posture_sub = rospy.Subscriber("/face_adls/%s_cart_posture" % arm_char,
Float32, self.adjust_posture_cb)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot([0]*3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name,
frame_rot, velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
self.controller_enabled_pub = rospy.Publisher('/face_adls/%s_cart_ctrl_enabled' % arm_char,
Bool, latch=True)
self.enable_controller_srv = rospy.Service("/face_adls/%s_enable_cart_ctrl" % arm_char,
EnableCartController, enable_controller_cb)
def enable_controller(self, end_link="%s_gripper_tool_frame",
ctrl_params="$(find hrl_face_adls)/params/%s_jt_task_tool.yaml",
ctrl_name="%s_cart_jt_task_tool",
frame_rot=FLIP_PERSPECTIVE_ROT, velocity=0.03):
#frame_rot=np.mat(np.eye(3))):
rospy.loginfo("[cartesian_manager] Enabling %s controller with end link %s" %
(ctrl_name, end_link))
try:
if '%s' in end_link:
end_link = end_link % self.arm_char
if '%s' in ctrl_params:
ctrl_params = ctrl_params % self.arm_char
if '%s' in ctrl_name:
ctrl_name = ctrl_name % self.arm_char
self.ctrl_switcher.carefree_switch(self.arm_char, ctrl_name, ctrl_params, reset=False)
rospy.sleep(0.2)
cart_arm = create_ep_arm(self.arm_char, PR2ArmJTransposeTask,
controller_name=ctrl_name,
end_link=end_link, timeout=5)
self.cart_ctrl.set_arm(cart_arm)
self.arm = cart_arm
rospy.sleep(0.1)
self.cur_posture_angle = self.arm.get_joint_angles()[2]
self.adjust_posture_cb(Float32(0.))
self.frame_rot = frame_rot
self.velocity = velocity
self.controller_enabled_pub.publish(True)
except Exception as e:
print e
return False
rospy.loginfo("[cartesian_manager] Successfully enabled %s controller with end link %s" %
(ctrl_name, end_link))
return True
def disable_controller(self):
rospy.loginfo("[cartesian_manager] Disabling cartesian controller.")
self.cart_ctrl.set_arm(None)
self.arm = None
self.controller_enabled_pub.publish(False)
return True
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(np.mat([0]*3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0),
velocity=self.velocity, blocking=True)
def command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1),
velocity=self.velocity, blocking=True)
def adjust_posture_cb(self, msg):
rospy.loginfo("[cartesian_manager] Adjusting posture")
new_posture = [None] * 7
self.cur_posture_angle += msg.data
new_posture[2] = self.cur_posture_angle
self.arm.set_posture(new_posture)
def main():
prefix = "/".join(sys.argv[0].split("/")[:-2]) + "/params/"
cs = ControllerSwitcher()
print cs.switch("r_arm_controller", "r_arm_controller", prefix + "pr2_arm_controllers_low.yaml")
class CartesianControllerManager(object):
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber(
"/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber(
"/face_adls/%s_cart_absolute" % arm_char, PoseStamped,
async_call(self.command_absolute_cb))
self.adjust_posture_sub = rospy.Subscriber(
"/face_adls/%s_cart_posture" % arm_char, Float32,
self.adjust_posture_cb)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot(
[0] * 3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link,
req.ctrl_params,
req.ctrl_name,
frame_rot,
velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
self.controller_enabled_pub = rospy.Publisher(
'/face_adls/%s_cart_ctrl_enabled' % arm_char, Bool, latch=True)
self.enable_controller_srv = rospy.Service(
"/face_adls/%s_enable_cart_ctrl" % arm_char, EnableCartController,
enable_controller_cb)
def enable_controller(
self,
end_link="%s_gripper_tool_frame",
ctrl_params="$(find hrl_face_adls)/params/%s_jt_task_tool.yaml",
ctrl_name="%s_cart_jt_task_tool",
frame_rot=FLIP_PERSPECTIVE_ROT,
velocity=0.03):
#frame_rot=np.mat(np.eye(3))):
rospy.loginfo(
"[cartesian_manager] Enabling %s controller with end link %s" %
(ctrl_name, end_link))
try:
if '%s' in end_link:
end_link = end_link % self.arm_char
if '%s' in ctrl_params:
ctrl_params = ctrl_params % self.arm_char
if '%s' in ctrl_name:
ctrl_name = ctrl_name % self.arm_char
self.ctrl_switcher.carefree_switch(self.arm_char,
ctrl_name,
ctrl_params,
reset=False)
rospy.sleep(0.2)
cart_arm = create_ep_arm(self.arm_char,
PR2ArmJTransposeTask,
controller_name=ctrl_name,
end_link=end_link,
timeout=5)
self.cart_ctrl.set_arm(cart_arm)
self.arm = cart_arm
rospy.sleep(0.1)
self.cur_posture_angle = self.arm.get_joint_angles()[2]
self.adjust_posture_cb(Float32(0.))
self.frame_rot = frame_rot
self.velocity = velocity
self.controller_enabled_pub.publish(True)
except Exception as e:
print e
return False
rospy.loginfo(
"[cartesian_manager] Successfully enabled %s controller with end link %s"
% (ctrl_name, end_link))
return True
def disable_controller(self):
rospy.loginfo("[cartesian_manager] Disabling cartesian controller.")
self.cart_ctrl.set_arm(None)
self.arm = None
self.controller_enabled_pub.publish(False)
return True
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(
np.mat([0] * 3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy),
((0, 0, 0), 0),
velocity=self.velocity,
blocking=True)
def command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot),
((1, 1, 1), 1),
velocity=self.velocity,
blocking=True)
def adjust_posture_cb(self, msg):
rospy.loginfo("[cartesian_manager] Adjusting posture")
new_posture = [None] * 7
self.cur_posture_angle += msg.data
new_posture[2] = self.cur_posture_angle
self.arm.set_posture(new_posture)
class ExperimentSetup(object):
def __init__(self):
self.ctrl_switcher = ControllerSwitcher()
self.torso_sac = actionlib.SimpleActionClient('torso_controller/position_joint_action',
SingleJointPositionAction)
self.torso_sac.wait_for_server()
self.head_point_sac = actionlib.SimpleActionClient(
'/head_traj_controller/point_head_action',
PointHeadAction)
self.head_point_sac.wait_for_server()
rospy.loginfo("[experiment_setup] ExperimentSetup ready.")
def point_head(self):
print "Pointing head"
head_goal = PointHeadGoal()
head_goal.target = PoseConverter.to_point_stamped_msg('/torso_lift_link',
np.mat([1., 0.4, 0.]).T,
np.mat(np.eye(3)))
head_goal.target.header.stamp = rospy.Time()
head_goal.min_duration = rospy.Duration(3.)
head_goal.max_velocity = 0.2
self.head_point_sac.send_goal_and_wait(head_goal)
def adjust_torso(self):
# move torso up
tgoal = SingleJointPositionGoal()
tgoal.position = 0.238 # all the way up is 0.300
tgoal.min_duration = rospy.Duration( 2.0 )
tgoal.max_velocity = 1.0
self.torso_sac.send_goal_and_wait(tgoal)
def mirror_arm_setup(self):
self.ctrl_switcher.carefree_switch('r', 'r_joint_controller_mirror',
"$(find hrl_ellipsoidal_control)/params/mirror_params.yaml")
rospy.sleep(1)
arm = create_pr2_arm('r', PR2ArmJointTrajectory,
controller_name="r_joint_controller_mirror")
arm.set_ep([-0.26880036055585677, 0.71881299774143248,
-0.010187938126536471, -1.43747589322259,
-12.531293698878677, -0.92339874393497123,
3.3566322715405432], 5)
rospy.sleep(6)
def tool_arm_setup(self):
self.ctrl_switcher.carefree_switch('l', '%s_arm_controller', None)
rospy.sleep(1)
joint_arm = create_pr2_arm('l', PR2ArmJointTrajectory)
setup_angles = SETUP_ANGLES
joint_arm.set_ep(setup_angles, 5)
rospy.sleep(6)
def mirror_mannequin(self):
arm = create_pr2_arm('r', PR2ArmJointTrajectory,
controller_name="r_joint_controller_mirror")
r = rospy.Rate(10)
q_act_last = arm.get_joint_angles()
while not rospy.is_shutdown():
q_act = arm.get_joint_angles()
q_ep = arm.get_ep()
new_ep = q_ep.copy()
for i in range(7):
if np.fabs(q_act[i] - q_act_last[i]) > joint_deltas[i]:
new_ep[i] = q_act[i]
arm.set_ep(new_ep, 0.1)
q_act_last = q_act
r.sleep()
def cart_controller_setup(self):
self.ctrl_switcher.carefree_switch('l', '%s_cart_jt_task',
"$(find hrl_rfh_fall_2011)/params/l_jt_task_shaver45.yaml")
self.cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask,
controller_name='%s_cart_jt_task',
end_link="%s_gripper_shaver45_frame")
rospy.sleep(2)
setup_angles = SETUP_ANGLES
self.cart_arm.set_posture(setup_angles)
self.cart_arm.set_gains([200, 800, 800, 80, 80, 80], [15, 15, 15, 1.2, 1.2, 1.2])
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l',
PR2ArmJTransposeTask,
end_link="%s_gripper_shaver45_frame")
class FaceADLsManager(object):
def __init__(self):
self.ellipsoid = EllipsoidSpace()
self.controller_switcher = ControllerSwitcher()
self.ee_frame = '/l_gripper_shaver305_frame'
self.head_pose = None
self.tfl = TransformListener()
self.is_forced_retreat = False
self.pose_traj_pub = rospy.Publisher('/haptic_mpc/goal_pose_array', PoseArray)
self.global_input_sub = rospy.Subscriber("/face_adls/global_move", String,
async_call(self.global_input_cb))
self.local_input_sub = rospy.Subscriber("/face_adls/local_move", String,
async_call(self.local_input_cb))
self.clicked_input_sub = rospy.Subscriber("/face_adls/clicked_move", PoseStamped,
async_call(self.global_input_cb))
self.feedback_pub = rospy.Publisher('/face_adls/feedback', String)
self.global_move_poses_pub = rospy.Publisher('/face_adls/global_move_poses',
StringArray, latch=True)
self.controller_enabled_pub = rospy.Publisher('/face_adls/controller_enabled',
Bool, latch=True)
self.enable_controller_srv = rospy.Service("/face_adls/enable_controller",
EnableFaceController, self.enable_controller_cb)
self.request_registration = rospy.ServiceProxy("/request_registration", RequestRegistration)
self.enable_mpc_srv = rospy.ServiceProxy('/haptic_mpc/enable_mpc', EnableHapticMPC)
self.ell_params_pub = rospy.Publisher("/ellipsoid_params", EllipsoidParams, latch=True)
def stop_move_cb(msg):
self.stop_moving()
self.stop_move_sub = rospy.Subscriber("/face_adls/stop_move", Bool, stop_move_cb, queue_size=1)
def stop_moving(self):
"""Send empty PoseArray to skin controller to stop movement"""
for x in range(2):
self.pose_traj_pub.publish(PoseArray()) #Send empty msg to skin controller
cart_traj_msg = [PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))]
head = Header()
head.frame_id = '/torso_lift_link'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
rospy.loginfo( "Sent stop moving commands!")
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now, rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame", now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" % (rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 * ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)
def current_ee_pose(self, link, frame='/torso_lift_link'):
"""Get current end effector pose from tf"""
# print "Getting pose of %s in frame: %s" %(link, frame)
try:
now = rospy.Time.now()
self.tfl.waitForTransform(frame, link, now, rospy.Duration(8.0))
pos, quat = self.tfl.lookupTransform(frame, link, now)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting current end-effector pose: %s" %e)
return None
return pos, quat
def publish_feedback(self, message=None):
if message is not None:
rospy.loginfo("[face_adls_manager] %s" % message)
self.feedback_pub.publish(message)
def enable_controller_cb(self, req):
if req.enable:
face_adls_modes = rospy.get_param('/face_adls_modes', None)
params = face_adls_modes[req.mode]
self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general
self.trim_retreat = req.mode == "shaving"
self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
bounds = params['%s_bounds' % self.face_side]
self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back
#self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))
success, e_params = self.register_ellipse(req.mode, self.face_side)
if not success:
self.publish_feedback(Messages.NO_PARAMS_LOADED)
return EnableFaceControllerResponse(False)
reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
try:
now = rospy.Time.now()
reg_pose.header.stamp = now
self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link',
now, rospy.Duration(8.0))
ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e)
self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
e_params.is_oblate, e_params.height)
global_poses_dir = rospy.get_param("~global_poses_dir", "")
global_poses_file = params["%s_ell_poses_file" % self.face_side]
global_poses_resolved = roslaunch.substitution_args.resolve_args(
global_poses_dir + "/" + global_poses_file)
self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
self.global_move_poses_pub.publish(sorted(self.global_poses.keys()))
#Check rotating gripper based on side of body
if self.flip_gripper:
self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
print "Rotating gripper by PI around x-axis"
else:
self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
print "NOT Rotating gripper around x-axis"
# check if arm is near head
# cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
# ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
# equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
# print ell_pos, equals
# if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
# arm_in_bounds = np.all(equals)
# else:
# ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
# min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
# arm_in_bounds = (equals[0] and
# equals[2] and
# (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
arm_in_bounds = True
self.setup_force_monitor()
success = True
if not arm_in_bounds:
self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
success = False
#Switch back to l_arm_controller if necessary
if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False):
print "Controller switch to l_arm_controller succeeded"
self.publish_feedback(Messages.ENABLE_CONTROLLER)
else:
print "Controller switch to l_arm_controller failed"
success = False
self.controller_enabled_pub.publish(Bool(success))
req = EnableHapticMPCRequest()
req.new_state = 'enabled'
resp = self.enable_mpc_srv.call(req)
else:
self.stop_moving()
self.controller_enabled_pub.publish(Bool(False))
rospy.loginfo(Messages.DISABLE_CONTROLLER)
req = EnableHapticMPCRequest()
req.new_state = 'disabled'
self.enable_mpc_srv.call(req)
success = False
return EnableFaceControllerResponse(success)
def setup_force_monitor(self):
self.force_monitor = ForceCollisionMonitor()
# registering force monitor callbacks
def dangerous_cb(force):
self.stop_moving()
curr_pose = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(curr_pose)
if ell_pos[2] < SAFETY_RETREAT_HEIGHT * 0.9:
self.publish_feedback(Messages.DANGEROUS_FORCE %force)
self.retreat_move(SAFETY_RETREAT_HEIGHT,
SAFETY_RETREAT_VELOCITY,
forced=True)
self.force_monitor.register_dangerous_cb(dangerous_cb)
def timeout_cb(time):
start_time = rospy.get_time()
ell_pos, _ = self.ellipsoid.pose_to_ellipsoidal(self.current_ee_pose(self.ee_frame, '/ellipse_frame'))
rospy.loginfo("ELL POS TIME: %.3f " % (rospy.get_time() - start_time) + str(ell_pos)
+ " times: %.3f %.3f" % (self.force_monitor.last_activity_time, rospy.get_time()))
if ell_pos[2] < RETREAT_HEIGHT * 0.9 and self.force_monitor.is_inactive():
self.publish_feedback(Messages.TIMEOUT_RETREAT % time)
self.retreat_move(RETREAT_HEIGHT, LOCAL_VELOCITY)
self.force_monitor.register_timeout_cb(timeout_cb)
def contact_cb(force):
self.force_monitor.update_activity()
if not self.is_forced_retreat:
self.stop_moving()
self.publish_feedback(Messages.CONTACT_FORCE % force)
rospy.loginfo("[face_adls_manZger] Arm stopped due to making contact.")
self.force_monitor.register_contact_cb(contact_cb)
self.force_monitor.start_activity()
self.force_monitor.update_activity()
self.is_forced_retreat = False
def retreat_move(self, height, velocity, forced=False):
if forced:
self.is_forced_retreat = True
cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
#print "Retreat EP:", ell_pos
latitude = ell_pos[0]
if self.trim_retreat:
latitude = min(latitude, TRIM_RETREAT_LATITUDE)
#rospy.loginfo("[face_adls_manager] Retreating from current location.")
retreat_pos = [latitude, ell_pos[1], height]
retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
retreat_quat = [0,0,0,1]
if forced:
cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
cart_msg = [PoseConv.to_pose_msg(cart_path)]
else:
ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
ell_quat,
retreat_pos,
retreat_quat,
velocity=0.01,
min_jerk=True)
cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_msg)
self.pose_traj_pub.publish(pose_array)
self.is_forced_retreat = False
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START %msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback('Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
retreat_ell_pos, retreat_ell_quat,
velocity=0.01, min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
approach_ell_pos, approach_ell_quat,
velocity=0.01, min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
final_ell_pos, final_ell_quat,
velocity=0.01, min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def local_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
self.stop_moving()
button_press = msg.data
curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
if button_press in ell_trans_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_trans_ep = ell_trans_params[button_press]
goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
curr_ell_pos[1]+change_trans_ep[1],
curr_ell_pos[2]+change_trans_ep[2]]
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
goal_ell_pos, curr_ell_quat,
velocity=ELL_LOCAL_VEL, min_jerk=True)
elif button_press in ell_rot_params:
self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
change_rot_ep = ell_rot_params[button_press]
rot_quat = trans.quaternion_from_euler(*change_rot_ep)
goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos, goal_ell_quat,
velocity=ELL_ROT_VEL, min_jerk=True)
elif button_press == "reset_rotation":
self.publish_feedback(Messages.ROT_RESET_START)
ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
curr_ell_pos,(0.,0.,0.,1.),
velocity=ELL_ROT_VEL, min_jerk=True)
else:
rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")
cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
self.force_monitor.update_activity()
def __init__(self):
self.tf = TransformListener()
self.jtarms = jttask_utils.jt_task_utils(self.tf)
self.controller_switcher = ControllerSwitcher()
##### Subscribers ####
self.disc_sub = rospy.Subscriber('wt_disc_shaving_step', Int8, self.disc_change_pose)
self.disc_sub.impl.add_callback(self.cancel_goals, None)
self.cont_sub = rospy.Subscriber('wt_cont_shaving_step', Point, self.cont_change_pose)
self.cont_sub.impl.add_callback(self.cancel_goals, None)
self.loc_sub = rospy.Subscriber('wt_shave_location', Int8, self.set_shave_pose)
self.loc_sub.impl.add_callback(self.cancel_goals, None)
rospy.Subscriber('wt_stop_shaving', Bool, self.stop_shaving)
#### Services ####
rospy.loginfo("Waiting for get_head_pose service")
try:
rospy.wait_for_service('/get_head_pose', 5.0)
self.get_pose_proxy = rospy.ServiceProxy('/get_head_pose', GetHeadPose)
rospy.loginfo("Found get_head_pose service")
except:
rospy.logwarn("get_head_pose service not available")
rospy.loginfo("Waiting for ellipsoid_command service")
try:
rospy.wait_for_service('/ellipsoid_command', 5.0)
self.ellipsoid_command_proxy = rospy.ServiceProxy('/ellipsoid_command', EllipsoidCommand)
rospy.loginfo("Found ellipsoid_command service")
except:
rospy.logwarn("ellipsoid_command service not available")
#### Publishers ####
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_out = rospy.Publisher('test_pose_1', PoseStamped)
self.rezero_wrench = rospy.Publisher('/netft_gravity_zeroing/rezero_wrench', Bool)
#### Data ####
self.hand_offset = 0.195
self.angle_tool_offset = 0.03
self.inline_tool_offset = 0.085
self.selected_pose = 0
self.poses = {
0 : ["near_ear",0.],
1 : ["upper_cheek",0.],
2 : ["middle_cheek",0.],
3 : ["jaw_bone",0.],
4 : ["back_neck",0.],
5 : ["nose",0.],
6 : ["chin",0.],
7 : ["mouth_corner", 0.]
}
#self.ft_wrench = WrenchStamped()
# self.force_unsafe = False
# self.ft_activity_thresh = 3
# self.ft_safety_thresh = 12
# rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.get_ft_state)
# rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_ft_state)
if POSTURE:
rospy.sleep(3)
print 'Sending Posture'
self.jtarms.send_posture(posture='elbowup', arm=0)
class ArmActions():
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy(
'/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point,
self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint,
self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.safe_approach)
#rospy.Subscriber('norm_approach_left', PoseStamped, self.hfc_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped,
self.force_wipe_agg)
rospy.Subscriber('wt_hfc_swipe_left_goals', PoseStamped,
self.hfc_swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32,
self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point,
self.force_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped,
self.approach_to_contact)
rospy.Subscriber('wt_hfc_contact_approach_left', PoseStamped,
self.hfc_approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
#rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('l_cart/ft_wrench', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_cont_switch', String, self.cont_switch)
#self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool)
self.rezero_wrench = rospy.Publisher('l_cart/ft_zero', Bool)
#rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
#rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
rospy.Subscriber('wt_ft_goal', Float32, self.hfc_set_force)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_mag_que = deque([0] * 10, 10)
self.ft_sm_mag = 0.
self.ft_case = None
self.force_goal_mean = 1.42
self.force_goal_std = 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
################### HFC ###################
rospy.Subscriber('wt_hfc_mannequin', Bool, self.hfc_mannequin)
self.active_cont = 'l_arm_controller'
self.cs = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
#kpt, kpr, kdt, kdr = 300, 8, 15, 4.2
#kfpt, kfpr, kfdt, kfdr = 3.8, 0.3, 0.1, 0.1 # force gains (derivative damps velocity)
#force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others
#self.arm.set_gains([kpt, kpt, kpt, kpr, kpr, kpr], [kdt, kdt, kdt, kdr, kdr, kdr], [kfpt, kfpt, kfpt, kfpr, kfpr, kfpr], [kfdt, kfdt, kfdt, kfdr, kfdr, kfdr], force_selector)
# force desired is currently at 0
self.hfc_pose_out = rospy.Publisher('/l_cart/command_pose',
PoseStamped)
rospy.Subscriber('l_cart/state/x', PoseStamped, self.hfc_cur_pose)
def cont_switch(self, string_msg):
if string_msg.data == 'l_arm_controller':
self.switch_to_default()
elif string_msg.data == 'l_cart':
self.switch_to_hfc()
else:
rospy.logerr("Controller Name Not Recognized")
def switch_to_hfc(self):
self.cs.switch('l_arm_controller', 'l_cart')
self.active_cont = 'l_cart'
rospy.loginfo("Switching to Hybrid Force Controller")
def switch_to_default(self):
self.cs.switch('l_cart', 'l_arm_controller')
self.active_cont = 'l_arm_controller'
rospy.loginfo("Switching to Default Joint Angle Controller")
def hfc_cur_pose(self, ps):
self.hfc_cur_pose = ps
def hfc_set_force(self, force):
self.arm.zero_sensor()
if type(force) == type(Float32()):
self.send_hfc_pose(self.hfc_cur_pose)
force = force.data
self.arm.set_force_directions([1, 0, 0, 0, 0, 0])
self.arm.update_gains()
self.arm.set_force([force, 0, 0, 0, 0, 0])
def send_hfc_pose(self, ps):
self.hfc_set_force(0.)
self.hfc_pose_out.publish(ps)
def hfc_slow_move(self, ps):
poses = self.aul.build_trajectory(ps, self.hfc_cur_pose)
pose_rate = rospy.Rate(4)
count = 0
self.hfc_set_force(0.)
self.arm.set_force_directions([0] * 6) # Turn off force control
self.arm.update_gains()
while count < len(poses):
#print count, poses[count]
self.hfc_pose_out.publish(poses[count])
count += 1
pose_rate.sleep()
def hfc_approach(self, ps):
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0)
#self.send_hfc_pose(appr)
self.hfc_slow_move(appr)
def hfc_mannequin(self, token):
self.arm.set_force([0, 0, 0, 0, 0, 0])
self.arm.set_force_directions([1, 1, 1, 1, 1, 1])
self.arm.update_gains()
def hfc_approach_to_contact(self, ps):
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0.1)
self.hfc_slow_move(appr)
dist = self.aul.calc_dist(self.hfc_cur_pose, appr)
#prep = self.safe_approach(ps)
if True: #prep:
self.switch_to_hfc()
self.arm.zero_sensor()
self.arm.set_force([1.6, 0, 0, 0, 0, 0])
self.arm.set_force_gains(p_trans=3.0)
self.arm.set_motion_gains(d_trans=1.0)
self.arm.set_force_directions([1, 0, 0, 0, 0, 0])
self.arm.update_gains()
def hfc_swipe(self, ps):
self.hfc_approach_to_contact(ps)
while self.ft.wrench.force.x < 1:
rospy.sleep(0.1)
self.arm.set_force([1.0, 0, 3.0, 0, 0, 0])
self.arm.set_force_directions([1, 0, 1, 0, 0, 0])
self.arm.update_gains()
############ END HFC #################
def set_force_goal(self, msg):
self.force_goal_mean = msg.data
def ft_preprocess(self, ft):
self.ft = ft
self.ft_mag = math.sqrt(ft.wrench.force.x**2 + ft.wrench.force.y**2 +
ft.wrench.force.z**2)
self.ft_mag_que.append(self.ft_mag)
self.ft_sm_mag = np.mean(self.ft_mag_que)
#print 'Force Magnitude: ', self.ft_mag
self.wt_force_out.publish(self.ft_mag)
def approach_to_contact(self, ps, overshoot=0.05):
ps.pose.position.z += 0.02
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0.15)
goal = self.aul.find_approach(ps, -overshoot)
(appr_reachable, ik_goal) = self.aul.full_ik_check(appr)
(goal_reachable, ik_goal) = self.aul.full_ik_check(goal)
if appr_reachable and goal_reachable:
traj = self.aul.build_trajectory(goal, appr, tot_points=1000)
#prep = self.aul.move_arm_to(appr)
self.aul.blind_move(appr)
if True: # if prep:
self.adjust_forearm(traj.points[0].positions)
self.rezero_wrench.publish(data=True)
curr_traj_point = self.advance_to_contact(traj)
if not curr_traj_point is None:
self.maintain_norm_force2(traj, curr_traj_point)
#self.maintain_force_position(self.aul.hand_frame_move(0.05))
#self.twist_wipe(); self.aul.blind_move(appr)
else:
self.aul.send_traj_point(traj.points[0], 4)
else:
rospy.loginfo("Cannot reach desired 'move-to-contact' point")
self.wt_log_out.publish(
data="Cannot reach desired 'move-to-contact' point")
def advance_to_contact(self, traj):
self.stop_maintain = False
curr_traj_point = 0
advance_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point], 0.01)
curr_traj_point += 1
advance_rate.sleep()
else:
rospy.loginfo(
"Moved past expected contact, but no contact found! Returning to start"
)
self.wt_log_out.publish(
data=
"Moved past expected contact, but no contact found! Returning to start"
)
self.stop_maintain = True
return None
if self.ft_mag > 1.5:
self.stop_maintain = True
print "Contact Detected"
return curr_traj_point
def maintain_norm_force2(self, traj, curr_traj_point=0, mean=0, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(1000)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft_mag > 12:
rospy.loginfo("Force Too High, ending behavior")
self.wt_log_out.publish(data="Force too high, ending behavior")
break
#print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag
if self.ft_mag < mean - std:
curr_traj_point += 1
curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points))
#print curr_traj_point
print "Low"
if not (curr_traj_point >= len(traj.points)):
self.aul.send_traj_point(traj.points[curr_traj_point],
0.01)
#else:
# rospy.loginfo("Force too low, but extent of the trajectory is reached")
# self.wt_log_out.publish(data="Force too low, but extent of the trajectory is reached")
# self.stop_maintain = True
elif self.ft_mag > mean + std:
print "High"
steps = int(round((self.ft_mag / std)))
curr_traj_point -= steps
#print curr_traj_point
curr_traj_point = np.clip(curr_traj_point, 0, len(traj.points))
if curr_traj_point >= 0:
self.aul.send_traj_point(traj.points[curr_traj_point],
0.009)
else:
rospy.loginfo(
"Beginning of trajectory reached, cannot back away further"
)
self.wt_log_out.publish(
data=
"Beginning of trajectory reached, cannot back away further"
)
#self.stop_maintain = True
maintain_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
print "Returning to start position"
self.aul.send_traj_point(traj.points[0], 4)
def maintain_norm_force(self, traj, curr_traj_point=0, mean=0, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(1000)
while not (rospy.is_shutdown() or self.stop_maintain):
#print "mean: ", mean, "stds: ", std, "force: ", self.ft_mag
if self.ft_mag < mean - std:
curr_traj_point += 1
np.clip(curr_traj_point, 0, len(traj.points))
if not (curr_traj_point >= len(traj.points)):
print curr_traj_point
self.aul.send_traj_point(traj.points[curr_traj_point],
0.0025)
#rospy.sleep(0.002)
else:
rospy.loginfo(
"Force too low, but extent of the trajectory is reached"
)
self.wt_log_out.publish(
data=
"Force too low, but extent of the trajectory is reached"
)
stopped = True
elif self.ft_mag > mean + std:
curr_traj_point -= 1
np.clip(curr_traj_point, 0, len(traj.points))
if curr_traj_point >= 0:
print curr_traj_point
self.aul.send_traj_point(traj.points[curr_traj_point],
0.0001)
#rospy.sleep(0.002)
else:
rospy.loginfo(
"Beginning of trajectory reached, cannot back away further"
)
self.wt_log_out.publish(
data=
"Beginning of trajectory reached, cannot back away further"
)
stopped = True
maintain_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
# def maintain_net_force(self, mean=0, std=3):
# self.stop_maintain = False
# maintain_rate = rospy.Rate(100)
# while not (rospy.is_shutdown() or self.stop_maintain):
# if self.ft_mag > mean + 8:
# curr_angs = self.aul.joint_state_act.positions
# try:
# x_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0.02))).solution.joint_state.position, curr_angs)
# x_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(-0.02))).solution.joint_state.position, curr_angs)
# y_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0.02, 0))).solution.joint_state.position, curr_angs)
# y_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, -0.02, 0))).solution.joint_state.position, curr_angs)
# z_plus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, 0.02))).solution.joint_state.position, curr_angs)
# z_minus = np.subtract(self.aul.ik_pose_proxy(self.aul.form_ik_request(self.aul.hand_frame_move(0, 0, -0.02))).solution.joint_state.position, curr_angs)
# #print 'x: ', x_plus,'\r\n', x_minus
# #print 'y: ', y_plus,'\r\n', y_minus
# #print 'z: ', z_plus,'\r\n', z_minus
# ft_sum = self.ft_mag
# parts = np.divide([self.ft.wrench.force.x, self.ft.wrench.force.y, self.ft.wrench.force.z], ft_sum)
# print 'parts', parts
# ends = [[x_plus,x_minus],[y_plus, y_minus],[z_plus,z_minus]]
# side = [[0]*7]*3
# for i, part in enumerate(parts):
# if part >=0:
# side[i] = ends[i][0]
# else:
# side[i] = ends[i][1]
#
# ang_out = curr_angs
# for i in range(3):
# ang_out -= np.average(side, 0, parts)
# except:
# print 'Near Joint Limits!'
# self.aul.send_joint_angles(ang_out)
#
# #print 'x: ', x_plus, x_minus
# maintain_rate.sleep()
# mean = self.force_goal_mean
# std = self.force_goal_std
def maintain_force_position(self, pose=None, mean=3, std=1):
self.stop_maintain = False
if pose is None:
goal = self.aul.curr_pose()
else:
goal = pose
goal_ort = [
goal.pose.orientation.x, goal.pose.orientation.y,
goal.pose.orientation.z, goal.pose.orientation.w
]
error = PoseStamped()
maintain_rate = rospy.Rate(100)
while not (rospy.is_shutdown() or self.stop_maintain):
current = self.aul.curr_pose()
current_ort = [
current.pose.orientation.x, current.pose.orientation.y,
current.pose.orientation.z, current.pose.orientation.w
]
error.pose.position.x = current.pose.position.x - goal.pose.position.x
error.pose.position.y = current.pose.position.y - goal.pose.position.y
error.pose.position.z = current.pose.position.z - goal.pose.position.z
error_mag = math.sqrt(error.pose.position.x**2 +
error.pose.position.y**2 +
error.pose.position.z**2)
out = deepcopy(goal)
out.header.stamp = rospy.Time.now()
if all(np.array(self.ft_mag_que) < mean -
std) and error_mag > 0.005:
print 'Force Too LOW'
out.pose.position.x += 0.990 * error.pose.position.x
out.pose.position.y += 0.990 * error.pose.position.y
out.pose.position.z += 0.990 * error.pose.position.z
ori = transformations.quaternion_slerp(goal_ort, current_ort,
0.990)
out.pose.orientation = Quaternion(*ori)
self.aul.fast_move(out, max(0.3 * error_mag, 0.05))
self.test_pose.publish(out)
elif all(np.array(self.ft_mag_que) > mean + std):
print 'Moving to avoid force'
current.pose.position.x += self.ft.wrench.force.x / 9000
current.pose.position.y += self.ft.wrench.force.y / 9000
current.pose.position.z += self.ft.wrench.force.z / 9000
self.aul.fast_move(current, 0.02)
self.test_pose.publish(current)
else:
print "Force in desired range"
mean = self.force_goal_mean
std = self.force_goal_std
rospy.sleep(0.001)
maintain_rate.sleep()
#def maintain_force_position(self,pose = None, mean=3, std=1):
# self.stop_maintain = False
# if pose is None:
# goal = self.aul.curr_pose()
# else:
# goal = pose
# self.rezero_wrench.publish(data=True)
# maintain_rate = rospy.Rate(500)
# kp = 0.07
# kd = 0.03
# ki = 0.0
# error = PoseStamped()
# old_error = PoseStamped()
# sum_error_x = deque([0]*10,10)
# sum_error_y = deque([0]*10,10)
# sum_error_z = deque([0]*10,10)
# while not (rospy.is_shutdown() or self.stop_maintain):
# current = self.aul.curr_pose()
# error.pose.position.x = current.pose.position.x - goal.pose.position.x
# error.pose.position.y = current.pose.position.y - goal.pose.position.y
# error.pose.position.z = current.pose.position.z - goal.pose.position.z
# sum_error_x.append(error.pose.position.x)
# sum_error_y.append(error.pose.position.y)
# sum_error_z.append(error.pose.position.z)
# print "Force: ", self.ft_sm_mag, min(self.ft_mag_que), max(self.ft_mag_que)
# print "Error: ", error.pose.position
# print self.ft_mag_que, mean-std
# print self.ft_mag_que < mean-std
# break
# if all([self.ft_mag_que < mean - std]):
# print 'Force Too LOW'
# current.pose.position.x += kp*error.pose.position.x + kd*(error.pose.position.x - old_error.pose.position.x) + ki*np.sum(sum_error_x)
# current.pose.position.x += kp*error.pose.position.y + kd*(error.pose.position.y - old_error.pose.position.y) + ki*np.sum(sum_error_y)
# current.pose.position.x += kp*error.pose.position.z + kd*(error.pose.position.z - old_error.pose.position.z) + ki*np.sum(sum_error_z)
# self.aul.fast_move(current, 0.02)
# self.test_pose.publish(current)
# if all([i > mean + std for i in self.ft_mag_que]):
# print 'Moving to avoid force'
# current.pose.position.x += self.ft.wrench.force.x/10000
# current.pose.position.y += self.ft.wrench.force.y/10000
# current.pose.position.z += self.ft.wrench.force.z/10000
# self.aul.fast_move(current, 0.02)
# self.test_pose.publish(current)
# old_error = error
# mean = self.force_goal_mean
# std = self.force_goal_std
# maintain_rate.sleep()
def maintain_net_force(self, mean=3, std=1):
self.stop_maintain = False
maintain_rate = rospy.Rate(500)
self.rezero_wrench.publish(data=True)
while not (rospy.is_shutdown() or self.stop_maintain):
if self.ft_mag > mean + 3:
print 'Moving to avoid force'
print "Force: ", self.ft_mag
goal = self.aul.curr_pose()
goal.pose.position.x += np.clip(self.ft.wrench.force.x / 5000,
-0.001, 0.001)
goal.pose.position.y += np.clip(self.ft.wrench.force.y / 5000,
-0.001, 0.001)
goal.pose.position.z += np.clip(self.ft.wrench.force.z / 5000,
-0.001, 0.001)
self.test_pose.publish(goal)
self.aul.fast_move(goal, 0.02)
mean = self.force_goal_mean
std = self.force_goal_std
maintain_rate.sleep()
def mannequin(self):
mannequin_rate = rospy.Rate(100)
pose = PoseStamped()
while not rospy.is_shutdown():
#joints = np.add(np.array(self.aul.joint_state_act.positions), np.clip(np.array(self.aul.joint_state_err.positions), -0.05, 0.05))
joints = self.aul.joint_state_act.positions
print joints
#raw_input('Review Joint Angles')
self.aul.send_joint_angles(joints, 0.00001)
pose.header.stamp = rospy.Time.now()
self.test_pose.publish(pose)
mannequin_rate.sleep()
def force_wipe_agg(self, ps):
ps.pose.position.z += 0.02
self.aul.update_frame(ps)
rospy.sleep(0.1)
pose = self.aul.find_approach(ps, 0)
(goal_reachable, ik_goal) = self.aul.ik_check(pose)
if goal_reachable:
if not self.force_wipe_started:
appr = self.aul.find_approach(ps, 0.20)
(appr_reachable, ik_goal) = self.aul.ik_check(appr)
self.test_pose.publish(appr)
if appr_reachable:
self.force_wipe_start = pose
self.force_wipe_appr = appr
self.force_wipe_started = True
else:
rospy.loginfo(
"Cannot reach approach point, please choose another")
self.wt_log_out.publish(
data=
"Cannot reach approach point, please choose another")
self.say.publish(
data=
"I cannot get to a safe approach for there, please choose another point"
)
else:
ps1, ps2 = self.align_poses(self.force_wipe_start, pose)
self.force_wipe_prep(ps1, ps2)
# self.force_wipe_prep(self.force_wipe_start, pose)
self.force_wipe_started = False
else:
rospy.loginfo("Cannot reach wiping point, please choose another")
self.wt_log_out.publish(
data="Cannot reach wiping point, please choose another")
self.say.publish(
data="I cannot reach there, please choose another point")
def force_wipe_prep(self, ps_start, ps_finish, travel=0.05):
ps_start.header.stamp = rospy.Time.now()
ps_finish.header.stamp = rospy.Time.now()
ps_start_far = self.aul.hand_frame_move(travel, 0, 0, ps_start)
ps_start_near = self.aul.hand_frame_move(-travel, 0, 0, ps_start)
ps_finish_far = self.aul.hand_frame_move(travel, 0, 0, ps_finish)
ps_finish_near = self.aul.hand_frame_move(-travel, 0, 0, ps_finish)
n_points = int(
math.ceil(self.aul.calc_dist(ps_start, ps_finish) * 9000))
print 'n_points: ', n_points
mid_traj = self.aul.build_trajectory(ps_finish,
ps_start,
tot_points=n_points,
jagged=False)
near_traj = self.aul.build_trajectory(ps_finish_near,
ps_start_near,
tot_points=n_points,
jagged=False)
far_traj = self.aul.build_trajectory(ps_finish_far,
ps_start_far,
tot_points=n_points,
jagged=False)
self.force_wipe(mid_traj, near_traj, far_traj)
def force_surf_wipe(self, point):
self.fsw_poses = self.prep_surf_wipe(point)
if not self.fsw_poses is None:
near_poses = far_poses = [
PoseStamped() for i in xrange(len(self.fsw_poses))
]
for i, p in enumerate(self.fsw_poses):
near_poses[i] = self.aul.pose_frame_move(p, -0.05)
far_poses[i] = self.aul.pose_frame_move(p, 0.05)
near_traj = self.aul.fill_ik_traj(near_poses)
mid_traj = self.aul.fill_ik_traj(self.fsw_poses)
far_traj = self.aul.fill_ik_traj(far_poses)
print 'Trajectories Found'
self.force_wipe(mid_traj, near_traj, far_traj)
def adjust_forearm(self, in_angles):
print 'cur angles: ', self.aul.joint_state_act.positions, 'angs: ', in_angles
print np.abs(np.subtract(self.aul.joint_state_act.positions,
in_angles))
if np.max(
np.abs(
np.subtract(self.aul.joint_state_act.positions,
in_angles))) > math.pi:
self.say.publish(data="Adjusting for-arm roll")
print "Evasive Action!"
angles = list(self.aul.joint_state_act.positions)
flex = in_angles[5]
angles[5] = -0.1
self.aul.send_joint_angles(angles, 4)
angles[4] = in_angles[4]
self.aul.send_joint_angles(angles, 6)
angles[5] = flex
self.aul.send_joint_angles(angles, 4)
def force_wipe(self, mid_traj, near_traj, far_traj):
near_angles = [
list(near_traj.points[i].positions)
for i in range(len(near_traj.points))
]
mid_angles = [
list(mid_traj.points[i].positions)
for i in range(len(mid_traj.points))
]
far_angles = [
list(far_traj.points[i].positions)
for i in range(len(far_traj.points))
]
print 'lens: nmf: ', len(near_angles), len(mid_angles), len(far_angles)
fmn_diff = np.abs(np.subtract(near_angles, far_angles))
fmn_max = np.max(fmn_diff, axis=0)
print 'fmn_max: ', fmn_max
if any(fmn_max > math.pi / 2):
rospy.loginfo(
"TOO LARGE OF AN ANGLE CHANGE ALONG GRADIENT, IK REGION PROBLEMS!"
)
self.wt_log_out.publish(
data=
"The path requested required large movements (IK Limits cause angle wrapping)"
)
self.say.publish(
data="Large motion detected, cancelling. Please try again.")
return
for i in range(7):
n_max = max(np.abs(np.diff(near_angles, axis=0)[i]))
m_max = max(np.abs(np.diff(mid_angles, axis=0)[i]))
f_max = max(np.abs(np.diff(far_angles, axis=0)[i]))
n_mean = 4 * np.mean(np.abs(np.diff(near_angles, axis=0)[i]))
m_mean = 4 * np.mean(np.abs(np.diff(mid_angles, axis=0)[i]))
f_mean = 4 * np.mean(np.abs(np.diff(far_angles, axis=0)[i]))
print 'near: max: ', n_max, 'mean: ', n_mean
print 'mid: max: ', m_max, 'mean: ', m_mean
print 'far: max: ', f_max, 'mean: ', f_mean
if (n_max >= n_mean) or (m_max >= m_mean) or (f_max >= f_mean):
rospy.logerr(
"TOO LARGE OF AN ANGLE CHANGE ALONG PATH, IK REGION PROBLEMS!"
)
self.wt_log_out.publish(
data=
"The path requested required large movements (IK Limits cause angle wrapping)"
)
self.say.publish(
data="Large motion detected, cancelling. Please try again."
)
return
near_diff = np.subtract(near_angles, mid_angles).tolist()
far_diff = np.subtract(far_angles, mid_angles).tolist()
self.say.publish(data="Moving to approach point")
appr = self.force_wipe_appr
appr.pose.orientation = self.aul.get_fk(
near_angles[0]).pose.orientation
#prep = self.aul.move_arm_to(appr)
self.aul.blind_move(appr)
#rospy.sleep(3)
if True: #prep:
self.adjust_forearm(near_angles[0])
self.say.publish(data="Making Approach.")
bias = 2
self.aul.send_joint_angles(
np.add(mid_angles[0], np.multiply(bias, near_diff[0])), 3.5)
self.rezero_wrench.publish(data=True)
rospy.sleep(1)
wipe_rate = rospy.Rate(1000)
self.stop_maintain = False
count = 0
lap = 0
max_laps = 4
mean = self.force_goal_mean
std = self.force_goal_std
self.say.publish(data="Wiping")
single_dir = False #True
time = rospy.Time.now().to_sec()
while not (rospy.is_shutdown() or self.stop_maintain) and (
count + 1 <= len(mid_angles)) and (lap < max_laps):
if self.ft_mag > 10:
angles_out = np.add(mid_angles[0],
np.multiply(2, near_diff[0]))
self.aul.send_joint_angles(angles_out, 2)
rospy.loginfo("Force Too High, ending behavior")
self.wt_log_out.publish(
data="Force too high, ending behavior")
break
#print "mean: ", mean, "std: ", std, "force: ", self.ft_mag
if self.ft_mag >= mean + std:
# print 'Force too high!'
bias += (self.ft_mag / 500)
elif self.ft_mag < mean - std:
# print 'Force too low'
bias -= max(0.003, (self.ft_mag / 1500))
else:
# print 'Force Within Range'
count += 1
bias = np.clip(bias, -1, 2)
if bias > 0.:
diff = near_diff[count]
else:
diff = far_diff[count]
angles_out = np.add(mid_angles[count],
np.multiply(abs(bias), diff))
self.aul.send_joint_angles(angles_out, 0.0025)
#rospy.sleep(0.0000i1)
#print "Rate: ", (1/ (rospy.Time.now().to_sec() - time))
#time = rospy.Time.now().to_sec()
wipe_rate.sleep()
mean = self.force_goal_mean
std = self.force_goal_std
if count + 1 >= len(mid_angles):
if single_dir:
bias = 1
pose = self.aul.curr_pose()
pose = self.aul.hand_frame_move(-0.01)
rot = transformations.quaternion_about_axis(
math.radians(-10), (0, 1, 0))
q = transformations.quaternion_multiply([
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
], rot)
pose.pose.orientation = Quaternion(*q)
self.aul.blind_move(pose)
#goal = self.aul.ik_pose_proxy(self.aul.form_ik_request(pose))
#if goal.error_code.val == 1:
# self.aul.send_angles_wrap(goal.solution.joint_state.position)
#angles_out = list(self.aul.joint_state_act.positions)
#angles_out[4] += 0.05
# self.aul.send_joint_angles(angles_out,3)
angles_out = np.add(
mid_angles[count],
np.multiply(bias, near_diff[count]))
self.aul.send_joint_angles(angles_out, 5)
angles_out = np.add(mid_angles[0],
np.multiply(bias, near_diff[0]))
self.aul.send_joint_angles(angles_out, 5)
else:
mid_angles.reverse()
near_diff.reverse()
far_diff.reverse()
lap += 1
#if lap == 3:
# self.say.publish(data="Hold Still, you rascal!")
count = 0
rospy.sleep(0.5)
self.say.publish(data="Pulling away")
angles_out = np.add(mid_angles[0], np.multiply(2, near_diff[0]))
self.aul.send_joint_angles(angles_out, 5)
rospy.sleep(5)
self.say.publish(data="Finished wiping. Thank you")
def torso_frame_move(self, msg):
self.stop_maintain = True
goal = self.aul.curr_pose()
goal.pose.position.x += msg.x
goal.pose.position.y += msg.y
goal.pose.position.z += msg.z
self.aul.blind_move(goal)
def hand_move(self, f32):
self.stop_maintain = True
hfm_pose = self.aul.hand_frame_move(f32.data)
self.aul.blind_move(hfm_pose)
def safe_approach(self, ps, standoff=0.15):
self.stop_maintain = True
print 'Controller: ', self.active_cont
if self.active_cont != 'l_arm_controller':
self.switch_to_default()
rospy.sleep(1)
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, standoff)
arrived = self.aul.move_arm_to(appr)
return arrived
def grasp(self, ps):
self.stop_maintain = True
rospy.loginfo("Initiating Grasp Sequence")
self.wt_log_out.publish(data="Initiating Grasp Sequence")
self.aul.update_frame(ps)
approach = self.aul.find_approach(ps, 0.15)
rospy.loginfo("approach: \r\n %s" % approach)
at_appr = self.aul.move_arm_to(approach)
rospy.loginfo("arrived at approach: %s" % at_appr)
if at_appr:
opened = self.aul.gripper(0.09)
if opened:
rospy.loginfo("making linear approach")
#hfm_pose = self.aul.hand_frame_move(0.23)
hfm_pose = self.aul.find_approach(ps, -0.02)
self.aul.blind_move(hfm_pose)
self.aul.wait_for_stop(2)
closed = self.aul.gripper(0.0)
if not closed:
rospy.loginfo(
"Couldn't close completely: Grasp likely successful")
hfm_pose = self.aul.hand_frame_move(-0.23)
self.aul.blind_move(hfm_pose)
else:
pass
def prep_surf_wipe(self, point):
pixel_u = point.x
pixel_v = point.y
test_pose = self.p23d_proxy(pixel_u, pixel_v).pixel3d
self.aul.update_frame(test_pose)
test_pose = self.aul.find_approach(test_pose, 0)
(reachable, ik_goal) = self.aul.full_ik_check(test_pose)
if reachable:
if not self.surf_wipe_started:
start_pose = test_pose
self.surf_wipe_start = [pixel_u, pixel_v, start_pose]
self.surf_wipe_started = True
rospy.loginfo(
"Received valid starting position for wiping action")
self.wt_log_out.publish(
data="Received valid starting position for wiping action")
return None #Return after 1st point, wait for second
else:
rospy.loginfo(
"Received valid ending position for wiping action")
self.wt_log_out.publish(
data="Received valid ending position for wiping action")
self.surf_wipe_started = False #Continue on successful 2nd point
else:
rospy.loginfo("Cannot reach wipe position, please try another")
self.wt_log_out.publish(
data="Cannot reach wipe position, please try another")
return None #Return on invalid point, wait for another
dist = self.aul.calc_dist(self.surf_wipe_start[2], test_pose)
print 'dist', dist
num_points = dist / 0.01
print 'num_points', num_points
us = np.round(np.linspace(self.surf_wipe_start[0], pixel_u,
num_points))
vs = np.round(np.linspace(self.surf_wipe_start[1], pixel_v,
num_points))
surf_points = [PoseStamped() for i in xrange(len(us))]
print "Surface Points", [us, vs]
for i in xrange(len(us)):
pose = self.p23d_proxy(us[i], vs[i]).pixel3d
self.aul.update_frame(pose)
surf_points[i] = self.aul.find_approach(pose, 0)
print i + 1, '/', len(us)
return surf_points
#self.aul.blind_move(surf_points[0])
#self.aul.wait_for_stop()
#for pose in surf_points:
# self.aul.blind_move(pose,2.5)
# rospy.sleep(2)
# #self.aul.wait_for_stop()
#self.aul.hand_frame_move(-0.1)
def twist_wipe(self):
angles = list(self.aul.joint_state_act.positions)
count = 0
while count < 3:
angles[6] = -6.7
self.aul.send_joint_angles(angles)
while self.aul.joint_state_act.positions[6] > -6.6:
rospy.sleep(0.1)
angles[6] = 0.8
self.aul.send_joint_angles(angles)
while self.aul.joint_state_act.positions[6] < 0.7:
rospy.sleep(0.1)
count += 1
def poke(self, ps):
self.stop_maintain = True
self.aul.update_frame(ps)
appr = self.aul.find_approach(ps, 0.15)
touch = self.aul.find_approach(ps, 0)
prepared = self.aul.move_arm_to(appr)
if prepared:
self.aul.blind_move(touch)
self.aul.wait_for_stop()
rospy.sleep(7)
self.aul.blind_move(appr)
def swipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 1)
def wipe(self, ps):
traj = self.prep_wipe(ps)
if traj is not None:
self.stop_maintain = True
self.wipe_move(traj, 4)
def prep_wipe(self, ps):
#print "Prep Wipe Received: %s" %pa
self.aul.update_frame(ps)
print "Updating frame to: %s \r\n" % ps
if not self.wipe_started:
self.wipe_appr_seed = ps
self.wipe_ends[0] = self.aul.find_approach(ps, 0)
print "wipe_end[0]: %s" % self.wipe_ends[0]
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[0])
if not reachable:
rospy.loginfo(
"Cannot find approach for initial wipe position, please try another"
)
self.wt_log_out.publish(
data=
"Cannot find approach for initial wipe position, please try another"
)
return None
else:
self.wipe_started = True
rospy.loginfo("Received starting position for wiping action")
self.wt_log_out.publish(
data="Received starting position for wiping action")
return None
else:
self.wipe_ends[1] = self.aul.find_approach(ps, 0)
self.wipe_ends.reverse()
(reachable, ik_goal) = self.aul.full_ik_check(self.wipe_ends[1])
if not reachable:
rospy.loginfo(
"Cannot find approach for final wipe position, please try another"
)
self.wt_log_out.publish(
data=
"Cannot find approach for final wipe position, please try another"
)
return None
else:
rospy.loginfo("Received End position for wiping action")
self.wt_log_out.publish(
data="Received End position for wiping action")
####### REMOVED AND REPLACED WITH ALIGN FUNCTION ##############
self.wipe_ends[0], self.wipe_ends[1] = self.align_poses(
self.wipe_ends[0], self.wipe_ends[1])
self.aul.update_frame(self.wipe_appr_seed)
appr = self.aul.find_approach(self.wipe_appr_seed, 0.15)
appr.pose.orientation = self.wipe_ends[1].pose.orientation
prepared = self.aul.move_arm_to(appr)
if prepared:
#self.aul.advance_to_contact()
self.aul.blind_move(self.wipe_ends[1])
traj = self.aul.build_trajectory(self.wipe_ends[1],
self.wipe_ends[0])
wipe_traj = self.aul.build_follow_trajectory(traj)
self.aul.wait_for_stop()
self.wipe_started = False
return wipe_traj
#self.wipe(wipe_traj)
else:
rospy.loginfo(
"Failure reaching start point, please try again")
self.wt_log_out.publish(
data="Failure reaching start point, please try again")
def align_poses(self, ps1, ps2):
self.aul.update_frame(ps1)
ps2.header.stamp = rospy.Time.now()
self.tfl.waitForTransform(ps2.header.frame_id, 'lh_utility_frame',
rospy.Time.now(), rospy.Duration(3.0))
ps2_in_ps1 = self.tfl.transformPose('lh_utility_frame', ps2)
ang = math.atan2(-ps2_in_ps1.pose.position.z,
-ps2_in_ps1.pose.position.y) + (math.pi / 2)
q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0))
q_st_new = transformations.quaternion_multiply([
ps1.pose.orientation.x, ps1.pose.orientation.y,
ps1.pose.orientation.z, ps1.pose.orientation.w
], q_st_rot)
ps1.pose.orientation = Quaternion(*q_st_new)
self.aul.update_frame(ps2)
ps1.header.stamp = rospy.Time.now()
self.tfl.waitForTransform(ps1.header.frame_id, 'lh_utility_frame',
rospy.Time.now(), rospy.Duration(3.0))
ps1_in_ps2 = self.tfl.transformPose('lh_utility_frame', ps1)
ang = math.atan2(ps1_in_ps2.pose.position.z,
ps1_in_ps2.pose.position.y) + (math.pi / 2)
q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0))
q_st_new = transformations.quaternion_multiply([
ps2.pose.orientation.x, ps2.pose.orientation.y,
ps2.pose.orientation.z, ps2.pose.orientation.w
], q_st_rot)
ps2.pose.orientation = Quaternion(*q_st_new)
return ps1, ps2
def wipe_move(self, traj_goal, passes=4):
times = []
for i in range(len(traj_goal.trajectory.points)):
times.append(traj_goal.trajectory.points[i].time_from_start)
count = 0
while count < passes:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
traj_goal.trajectory.header.stamp = rospy.Time.now()
assert traj_goal.trajectory.points[0] != []
self.aul.l_arm_follow_traj_client.send_goal(traj_goal)
self.aul.l_arm_follow_traj_client.wait_for_result(
rospy.Duration(20))
rospy.sleep(0.5) # Pause at end of swipe
count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
hfm_pose = self.aul.hand_frame_move(-0.15)
self.aul.blind_move(hfm_pose)
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.ctrl_switcher = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy('/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point, self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint, self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.safe_approach)
#rospy.Subscriber('norm_approach_left', PoseStamped, self.hfc_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.force_wipe_agg)
rospy.Subscriber('wt_hfc_swipe_left_goals', PoseStamped, self.hfc_swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32, self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point, self.force_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped, self.approach_to_contact)
rospy.Subscriber('wt_hfc_contact_approach_left', PoseStamped, self.hfc_approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
#rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('l_cart/ft_wrench', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_cont_switch', String, self.cont_switch)
#self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool)
self.rezero_wrench = rospy.Publisher('l_cart/ft_zero', Bool)
#rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
#rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
rospy.Subscriber('wt_ft_goal', Float32, self.hfc_set_force)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_mag_que = deque([0]*10,10)
self.ft_sm_mag = 0.
self.ft_case = None
self.force_goal_mean = 1.42
self.force_goal_std= 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
################### HFC ###################
rospy.Subscriber('wt_hfc_mannequin', Bool, self.hfc_mannequin)
self.active_cont = 'l_arm_controller'
self.cs = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
#kpt, kpr, kdt, kdr = 300, 8, 15, 4.2
#kfpt, kfpr, kfdt, kfdr = 3.8, 0.3, 0.1, 0.1 # force gains (derivative damps velocity)
#force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others
#self.arm.set_gains([kpt, kpt, kpt, kpr, kpr, kpr], [kdt, kdt, kdt, kdr, kdr, kdr], [kfpt, kfpt, kfpt, kfpr, kfpr, kfpr], [kfdt, kfdt, kfdt, kfdr, kfdr, kfdr], force_selector)
# force desired is currently at 0
self.hfc_pose_out = rospy.Publisher('/l_cart/command_pose', PoseStamped)
rospy.Subscriber('l_cart/state/x', PoseStamped, self.hfc_cur_pose)
def main():
from optparse import OptionParser
p = OptionParser()
p.add_option('-f', '--filename', dest="filename", default="",
help="File to save trajectory to or load from.")
p.add_option('-l', '--left_arm', dest="left_arm",
action="store_true", default=False,
help="Use left arm.")
p.add_option('-r', '--right_arm', dest="right_arm",
action="store_true", default=False,
help="Use right arm.")
p.add_option('-s', '--save_mode', dest="save_mode",
action="store_true", default=False,
help="Saving mode.")
p.add_option('-t', '--traj_mode', dest="traj_mode",
action="store_true", default=False,
help="Trajectory mode.")
p.add_option('-c', '--ctrl_name', dest="ctrl_name", default=CTRL_NAME_LOW,
help="Controller to run the playback with.")
p.add_option('-p', '--params', dest="param_file", default=None, #PARAM_FILE_LOW,
help="YAML file to save parameters in or load from.")
p.add_option('-v', '--srv_mode', dest="srv_mode",
action="store_true", default=False,
help="Server mode.")
p.add_option('-y', '--playback_mode', dest="playback_mode",
action="store_true", default=False,
help="Plays back trajectory immediately.")
p.add_option('-z', '--reverse', dest="reverse",
action="store_true", default=False,
help="Plays back trajectory in reverse.")
(opts, args) = p.parse_args()
if opts.right_arm:
arm_char = 'r'
elif opts.left_arm:
arm_char = 'l'
else:
print "Must select arm with -r or -l."
return
filename = opts.filename
rospy.init_node("arm_pose_move_controller_%s" % arm_char)
if opts.save_mode:
assert(opts.right_arm + opts.left_arm == 1)
if opts.traj_mode:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(arm_char, CTRL_NAME_NONE, PARAM_FILE_NONE, reset=False)
traj_saver = TrajectorySaver(RATE, CTRL_NAME_NONE)
raw_input("Press enter to start recording")
traj_saver.record_trajectory(arm_char, blocking=False)
import curses
def wait_for_key(stdscr):
curses.init_pair(1, curses.COLOR_RED, curses.COLOR_WHITE)
stdscr.addstr(0,0, "Press any key to stop!", curses.color_pair(1) )
stdscr.refresh()
c = 0
while not rospy.is_shutdown() and c == 0:
c = stdscr.getch()
curses.wrapper(wait_for_key)
traj_saver.stop_record(roslaunch.substitution_args.resolve_args(opts.filename))
ctrl_switcher.carefree_switch(arm_char, opts.ctrl_name, PARAM_FILE_LOW, reset=False)
return
else:
print "FIX"
return
elif opts.srv_mode:
traj_srv = TrajectoryServer(arm_char, "/trajectory_playback_" + arm_char,
opts.ctrl_name, opts.param_file)
rospy.spin()
return
elif opts.playback_mode:
raw_input("Press enter to continue")
if opts.traj_mode:
exec_traj_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
else:
move_to_setup_from_file(opts.filename,
ctrl_name=opts.ctrl_name,
param_file=opts.param_file,
reverse=opts.reverse,
blocking=True)
def __init__(self):
rospy.init_node('left_arm_actions')
self.tfl = TransformListener()
self.aul = l_arm_utils.ArmUtils(self.tfl)
#self.fth = ft_handler.FTHandler()
rospy.loginfo("Waiting for l_utility_frame_service")
try:
rospy.wait_for_service('/l_utility_frame_update', 7.0)
self.aul.update_frame = rospy.ServiceProxy(
'/l_utility_frame_update', FrameUpdate)
rospy.loginfo("Found l_utility_frame_service")
except:
rospy.logwarn("Left Utility Frame Service Not available")
rospy.loginfo('Waiting for Pixel_2_3d Service')
try:
rospy.wait_for_service('/pixel_2_3d', 7.0)
self.p23d_proxy = rospy.ServiceProxy('/pixel_2_3d', Pixel23d, True)
rospy.loginfo("Found pixel_2_3d service")
except:
rospy.logwarn("Pixel_2_3d Service Not available")
#Low-level motion requests: pass directly to arm_utils
rospy.Subscriber('wt_left_arm_pose_commands', Point,
self.torso_frame_move)
rospy.Subscriber('wt_left_arm_angle_commands', JointTrajectoryPoint,
self.aul.send_traj_point)
#More complex motion scripts, defined here using arm_util functions
rospy.Subscriber('norm_approach_left', PoseStamped, self.safe_approach)
#rospy.Subscriber('norm_approach_left', PoseStamped, self.hfc_approach)
rospy.Subscriber('wt_grasp_left_goal', PoseStamped, self.grasp)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped, self.wipe)
rospy.Subscriber('wt_wipe_left_goals', PoseStamped,
self.force_wipe_agg)
rospy.Subscriber('wt_hfc_swipe_left_goals', PoseStamped,
self.hfc_swipe)
rospy.Subscriber('wt_lin_move_left', Float32, self.hand_move)
rospy.Subscriber('wt_adjust_elbow_left', Float32,
self.aul.adjust_elbow)
rospy.Subscriber('wt_surf_wipe_left_points', Point,
self.force_surf_wipe)
rospy.Subscriber('wt_poke_left_point', PoseStamped, self.poke)
rospy.Subscriber('wt_contact_approach_left', PoseStamped,
self.approach_to_contact)
rospy.Subscriber('wt_hfc_contact_approach_left', PoseStamped,
self.hfc_approach_to_contact)
self.wt_log_out = rospy.Publisher('wt_log_out', String)
self.test_pose = rospy.Publisher('test_pose', PoseStamped, latch=True)
self.say = rospy.Publisher('text_to_say', String)
self.wipe_started = False
self.surf_wipe_started = False
self.wipe_ends = [PoseStamped(), PoseStamped()]
#FORCE_TORQUE ADDITIONS
#rospy.Subscriber('netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('l_cart/ft_wrench', WrenchStamped, self.ft_preprocess)
rospy.Subscriber('wt_cont_switch', String, self.cont_switch)
#self.rezero_wrench = rospy.Publisher('netft_gravity_zeroing/rezero_wrench', Bool)
self.rezero_wrench = rospy.Publisher('l_cart/ft_zero', Bool)
#rospy.Subscriber('ft_data_pm_adjusted', WrenchStamped, self.ft_preprocess)
#rospy.Subscriber('wt_ft_goal', Float32, self.set_force_goal)
rospy.Subscriber('wt_ft_goal', Float32, self.hfc_set_force)
self.wt_force_out = rospy.Publisher('wt_force_out', Float32)
self.ft_rate_limit = rospy.Rate(30)
self.ft = WrenchStamped()
self.ft_mag = 0.
self.ft_mag_que = deque([0] * 10, 10)
self.ft_sm_mag = 0.
self.ft_case = None
self.force_goal_mean = 1.42
self.force_goal_std = 0.625
self.stop_maintain = False
self.force_wipe_started = False
self.force_wipe_start = PoseStamped()
self.force_wipe_appr = PoseStamped()
################### HFC ###################
rospy.Subscriber('wt_hfc_mannequin', Bool, self.hfc_mannequin)
self.active_cont = 'l_arm_controller'
self.cs = ControllerSwitcher()
self.arm = create_pr2_arm('l', PR2ArmHybridForce)
# motion gains p - proportional, d - derivative
# t - translational, r - rotational
#kpt, kpr, kdt, kdr = 300, 8, 15, 4.2
#kfpt, kfpr, kfdt, kfdr = 3.8, 0.3, 0.1, 0.1 # force gains (derivative damps velocity)
#force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others
#self.arm.set_gains([kpt, kpt, kpt, kpr, kpr, kpr], [kdt, kdt, kdt, kdr, kdr, kdr], [kfpt, kfpt, kfpt, kfpr, kfpr, kfpr], [kfdt, kfdt, kfdt, kfdr, kfdr, kfdr], force_selector)
# force desired is currently at 0
self.hfc_pose_out = rospy.Publisher('/l_cart/command_pose',
PoseStamped)
rospy.Subscriber('l_cart/state/x', PoseStamped, self.hfc_cur_pose)
