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")
