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 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 __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 __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("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("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 __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): 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("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 __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 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")
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(): 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): smach.State.__init__(self, outcomes=['succeeded']) self.ctrl_switcher = ControllerSwitcher() self.arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame")