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): rospy.init_node("ar_manipulation") # rospy.Subscriber("/ar_pose_markers", ARMarkers, self.read_markers_cb) # rospy.Subscriber("/ar_object_name", String, self.marker_lookup_cb) rospy.Subscriber("/ar_object_name", StringArray, self.marker_lookup_cb) rospy.Subscriber("/put_back_tool", String, self.put_back_tool_cb) self.pub_rate = rospy.Rate(10) self.torso = torso() self.head = head() self.gripper = gripper() self.tf_listener = tf.TransformListener() self.cs = pr2cs.ControllerSwitcher() self.pr2_init = False self.search_tag = False self.found_tag = False self.grasp_object = False # Load JTcontroller self.r_arm_cart = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJTranspose, controller_name="%s_cart", timeout=0) self.l_arm_cart = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJTranspose, controller_name="%s_cart", timeout=0) # Load Joint space controller self.r_arm = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJointTrajectory, controller_name="%s_arm_controller", timeout=0) self.l_arm = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJointTrajectory, controller_name="%s_arm_controller", timeout=0) self.epc = eptc.EPC('linear_move') self.time_step = 1/20.
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 __init__(self): rospy.init_node("ar_manipulation") # rospy.Subscriber("/ar_pose_markers", ARMarkers, self.read_markers_cb) # rospy.Subscriber("/ar_object_name", String, self.marker_lookup_cb) rospy.Subscriber("/ar_object_name", StringArray, self.marker_lookup_cb) rospy.Subscriber("/put_back_tool", String, self.put_back_tool_cb) self.pub_rate = rospy.Rate(10) self.torso = torso() self.head = head() self.gripper = gripper() self.tf_listener = tf.TransformListener() self.cs = pr2cs.ControllerSwitcher() self.pr2_init = False self.search_tag = False self.found_tag = False self.grasp_object = False # Load JTcontroller self.r_arm_cart = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJTranspose) self.l_arm_cart = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJTranspose) # Load Joint space controller self.r_arm = pr2arm.create_pr2_arm('r', pr2arm.PR2ArmJointTrajectory) self.l_arm = pr2arm.create_pr2_arm('l', pr2arm.PR2ArmJointTrajectory) self.epc = eptc.EPC('linear_move') self.time_step = 1/20.
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("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, min_l_torques=[-5.]*7, min_r_torques=[-5.]*7, max_l_torques=[5.]*7, max_r_torques=[5.]*7): smach.State.__init__(self, outcomes=['collision', 'preempted', 'aborted']) self.min_l_tor = np.array(min_l_torques) self.min_r_tor = np.array(min_r_torques) self.max_l_tor = np.array(max_l_torques) self.max_r_tor = np.array(max_r_torques) self.l_arm = create_pr2_arm('l', PR2ArmJointTrajectory, timeout=0) self.r_arm = create_pr2_arm('r', PR2ArmJointTrajectory, timeout=0)
def __init__(self, min_l_torques=[-5.] * 7, min_r_torques=[-5.] * 7, max_l_torques=[5.] * 7, max_r_torques=[5.] * 7): smach.State.__init__(self, outcomes=['collision', 'preempted', 'aborted']) self.min_l_tor = np.array(min_l_torques) self.min_r_tor = np.array(min_r_torques) self.max_l_tor = np.array(max_l_torques) self.max_r_tor = np.array(max_r_torques) self.l_arm = create_pr2_arm('l', PR2ArmJointTrajectory) self.r_arm = create_pr2_arm('r', PR2ArmJointTrajectory)
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()
def __init__(self, interface_backend): self.backend = interface_backend() self.cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, controller_name='%s_cart_jt_task', end_link="%s_gripper_shaver45_frame") self.backend.set_arm(self.cart_arm) rospy.loginfo("[ell_control_experiment] EllipsoidControlExperiment ready.")
def pose_saver(tf_frame, bag_file, pickle_file): arm = create_pr2_arm('l', PR2ArmJTransposeTask) tf_list = tf.TransformListener() named_poses = NamedPoses() named_poses.pose_arr.header.frame_id = tf_frame pose_dict = {} while True: if rospy.is_shutdown(): return pose_name = raw_input( "Type the name of the pose and hit enter to save it (Type 'q' to quit) " ) if pose_name == 'q': break arm_pose = PoseConverter.to_pose_stamped_msg( "/torso_lift_link", arm.get_end_effector_pose()) tf_pose = tf_list.transformPose(tf_frame, arm_pose) named_poses.pose_arr.poses.append(tf_pose) named_poses.names.append(pose_name) pose_dict[pose_name] = tf_pose if bag_file != "": bag = rosbag.Bag(bag_file, 'w') bag.write("/named_poses", named_poses) bag.close() if pickle_file != "": f = file(pickle_file, 'w') pickle.dump(pose_dict, f) f.close()
def pose_saver(tf_frame, bag_file, pickle_file): arm = create_pr2_arm('l', PR2ArmJTransposeTask) tf_list = tf.TransformListener() named_poses = NamedPoses() named_poses.pose_arr.header.frame_id = tf_frame pose_dict = {} while True: if rospy.is_shutdown(): return pose_name = raw_input("Type the name of the pose and hit enter to save it (Type 'q' to quit) ") if pose_name == 'q': break arm_pose = PoseConverter.to_pose_stamped_msg("/torso_lift_link", arm.get_end_effector_pose()) tf_pose = tf_list.transformPose(tf_frame, arm_pose) named_poses.pose_arr.poses.append(tf_pose) named_poses.names.append(pose_name) pose_dict[pose_name] = tf_pose if bag_file != "": bag = rosbag.Bag(bag_file, 'w') bag.write("/named_poses", named_poses) bag.close() if pickle_file != "": f = file(pickle_file, 'w') pickle.dump(pose_dict, f) f.close()
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 __init__(self): self.tf_listener = tf.TransformListener() self.start_frame_pub = rospy.Publisher("~start_frame", PoseStamped) self.end_frame_pub = rospy.Publisher("~end_frame", PoseStamped) self.arm = create_pr2_arm('l', PR2ArmHybridForce) self.tf_pub = TFPubLoop("/torso_lift_link", "/contact_control_frame")
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, interface_backend): self.backend = interface_backend() self.cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, controller_name='%s_cart_jt_task', end_link="%s_gripper_shaver45_frame") self.backend.set_arm(self.cart_arm) rospy.loginfo( "[ell_control_experiment] EllipsoidControlExperiment ready.")
def main(): rospy.init_node("teleop_positioner") from optparse import OptionParser p = OptionParser() p.add_option('-r', '--rate', dest="rate", default=10, help="Set rate.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(0.1) arm.zero_sensor() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) arm.set_force_directions([]) arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.5, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[10, 10, 10], d_rot=0) arm.set_tip_frame("/l_gripper_tool_frame") arm.update_gains() arm.set_force(6 * [0]) r = rospy.Rate(float(opts.rate)) while not rospy.is_shutdown(): ep_pose = arm.get_ep() cur_pose = arm.get_end_effector_pose() err_ep = arm.ep_error(cur_pose, ep_pose) if np.linalg.norm(err_ep[0:3]) > 0.012 or np.linalg.norm( err_ep[3:]) > np.pi / 8.: arm.set_ep(cur_pose, 1) r.sleep() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) q = arm.get_joint_angles() q_posture = q.tolist()[0:3] + 4 * [9999] arm.set_posture(q_posture) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[20, 50, 50], d_rot=0) arm.update_gains() print PoseConverter.to_pos_quat(cur_pose) pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"]) bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'w') bag.write("/teleop_pose", PoseConverter.to_pose_msg(cur_pose)) q_posture_msg = Float64MultiArray() q_posture_msg.data = q_posture bag.write("/teleop_posture", q_posture_msg) bag.close()
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 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 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 main(): rospy.init_node("ellipsoid_controller", sys.argv) if sys.argv[1] == '-s': setup_task_controller = SetupTaskController() setup_task_controller.execute(None) cart_arm = setup_task_controller.arm else: cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, end_link="%s_gripper_shaver45_frame") rospy.sleep(1) ell_controller = EllipsoidController(cart_arm) rospy.spin()
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 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("teleop_positioner") from optparse import OptionParser p = OptionParser() p.add_option('-r', '--rate', dest="rate", default=10, help="Set rate.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(0.1) arm.zero_sensor() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) arm.set_force_directions([]) arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.5, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[10, 10, 10], d_rot=0) arm.set_tip_frame("/l_gripper_tool_frame") arm.update_gains() arm.set_force(6 * [0]) r = rospy.Rate(float(opts.rate)) while not rospy.is_shutdown(): ep_pose = arm.get_ep() cur_pose = arm.get_end_effector_pose() err_ep = arm.ep_error(cur_pose, ep_pose) if np.linalg.norm(err_ep[0:3]) > 0.012 or np.linalg.norm(err_ep[3:]) > np.pi / 8.: arm.set_ep(cur_pose, 1) r.sleep() cur_pose = arm.get_end_effector_pose() arm.set_ep(cur_pose, 1) q = arm.get_joint_angles() q_posture = q.tolist()[0:3] + 4 * [9999] arm.set_posture(q_posture) arm.set_motion_gains(p_trans=400, d_trans=[16, 10, 10], p_rot=[20, 50, 50], d_rot=0) arm.update_gains() print PoseConverter.to_pos_quat(cur_pose) pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"]) bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'w') bag.write("/teleop_pose", PoseConverter.to_pose_msg(cur_pose)) q_posture_msg = Float64MultiArray() q_posture_msg.data = q_posture bag.write("/teleop_posture", q_posture_msg) bag.close()
def main(): rospy.init_node("pr2_arm_test") arm = sys.argv[1] jnt_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory) kin = jnt_arm.kinematics ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]]) sspace = SpheroidSpace(0.15, np.mat([0.78, -0.18, 0.1]).T, ellipse_rot) uvp = np.array([1.0, np.pi/2, 0.0]) uvp_delta = np.array([0.0, 0.6, 0.6]) pos, rot = sspace.spheroidal_to_pose(uvp + uvp_delta) print pos, rot #q_ik = kin.IK_search(pos, rot) q_ik = kin.IK(pos, rot, jnt_arm.get_joint_angles()) if q_ik is not None: jnt_arm.set_ep(q_ik, 5) else: print "IK failure"
def collect_data_pr2(n_4, n_5, n_6): roslib.load_manifest("hrl_pr2_arms") from hrl_pr2_arms.pr2_arm import PR2ArmJointTrajectory, create_pr2_arm netft_topic = rospy.get_param("~netft_topic") gravity_frame = rospy.get_param("~gravity_frame") netft_frame = rospy.get_param("~netft_frame") jnt_arm = create_pr2_arm('l', arm_type=PR2ArmJointTrajectory) kin = jnt_arm.kinematics netft_list = NetFTListener(netft_topic) tf_list = tf.TransformListener() q_setup = [0.4, 0.0, 1.0, -1.2, 0, -0.1, -3.14] q_4_vals = np.linspace(0, 4.7, n_4) q_5_vals = np.linspace(-0.1, -1.8, n_5) q_6_vals = np.linspace(-3.14, -0.2, n_6) jnt_arm.set_ep(q_setup, 8) rospy.sleep(8.) data = [] q_cur = q_setup for q_4 in q_4_vals: for q_5 in q_5_vals: for q_6 in q_6_vals: q_cur[4] = q_4 q_cur[5] = q_5 q_cur[6] = q_6 jnt_arm.set_ep(q_cur, 2.) rospy.sleep(4.) (trans, rot) = tf_list.lookupTransform(gravity_frame, netft_frame, rospy.Time(0)) data.append((copy.copy(netft_list.cur_wrench), rot)) if rospy.is_shutdown(): return print "Wrench, orientation data", data return data
def main(): rospy.init_node("cool_force_demo") from optparse import OptionParser p = OptionParser() p.add_option('-f', '--force_mag', dest="force_mag", default=2, help="Specify force magnitude.") p.add_option('-x', '--max_force', dest="max_force", default=-1, help="Specify max force magnitude.") p.add_option('-c', '--compliance', dest="compliance", default=-1, help="Compliance to maintain.") p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame", help="Set tip to this frame.") p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False, action="store_true", help="Just zero the sensor.") p.add_option('-l', '--force_line', dest="force_line", default=False, action="store_true", help="Move in a line with zero force.") p.add_option('-p', '--force_plane', dest="force_plane", default=False, action="store_true", help="Move in a plane with zero force.") p.add_option('-o', '--force_point', dest="force_point", default=False, action="store_true", help="Move about a point with zero torque.") p.add_option('-r', '--force_roll', dest="force_roll", default=False, action="store_true", help="Move the wrist with zero torque.") p.add_option('-a', '--force_all', dest="force_all", default=False, action="store_true", help="All DOF are trying to set zero force.") p.add_option('-n', '--force_none', dest="force_none", default=False, action="store_true", help="Return to position control.") p.add_option('-k', '--kill_controller', dest="kill_controller", default=False, action="store_true", help="Render controller dead.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(1) if opts.zero_sensor: arm.zero_sensor() # reset arm arm.set_ep(arm.get_end_effector_pose(), 0) arm.set_force(6 * [0]) fp_trans_major = 3 fp_trans_minor = 1 fp_rot = 0.1 fi_trans_major = 0.002 fi_trans_minor = 0.001 fi_max_trans_major = 10 fi_max_trans_minor = 5 fi_rot = 0 fi_max_rot = 0 pp_trans = 1000 pd_trans_major = 16 pd_trans_minor = 4 pp_rot = 120 pd_rot = 0 if opts.force_line: arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[fp_trans_major, fp_trans_minor, fp_trans_minor], p_rot=fp_rot, i_trans=[fi_trans_major, fi_trans_minor, fi_trans_minor], i_max_trans=[fi_max_trans_major, fi_max_trans_minor, fi_max_trans_minor], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains(p_trans=[float(opts.compliance), pp_trans, pp_trans], d_trans=[pd_trans_major, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions(['x']) arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0]) arm.update_gains() return if opts.force_plane: arm.set_force_max([-1, float(opts.max_force), float(opts.max_force), -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_major, fp_trans_major], p_rot=fp_rot, i_trans=[fi_trans_minor, fi_trans_major, fi_trans_major], i_max_trans=[fi_max_trans_minor, fi_max_trans_major, fi_max_trans_major], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains(p_trans=[pp_trans, float(opts.compliance), float(opts.compliance)], d_trans=[pd_trans_minor, pd_trans_major, pd_trans_major], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions(['y', 'z']) arm.set_force([0, float(opts.force_mag), float(opts.force_mag), 0, 0, 0]) arm.update_gains() return if opts.force_point: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor], p_rot=0.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor], i_max_trans=[fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([0, 0, 0, 1, 1, 1]) arm.set_force([0, 0, 0, float(opts.force_mag), float(opts.force_mag), float(opts.force_mag)]) arm.update_gains() return if opts.force_roll: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor], p_rot=1.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor], i_max_trans=[fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([0, 0, 0, 1, 0, 0]) arm.set_force([0, 0, 0, float(opts.force_mag), 0, 0]) arm.update_gains() return if opts.force_all: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=6, p_rot=1.8, i_trans=[fi_trans_major, fi_trans_major, fi_trans_major], i_max_trans=[fi_max_trans_major, fi_max_trans_major, fi_max_trans_major], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_major, pd_trans_major, pd_trans_major], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([1, 1, 1, 1, 1, 1]) arm.set_force([0, 0, 0, 0, 0, 0]) arm.update_gains() return if opts.force_none: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[0, 0, 0], p_rot=0, i_trans=[0, 0, 0], i_max_trans=[0, 0, 0], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([0, 0, 0, 0, 0, 0]) arm.set_force([0, 0, 0, 0, 0, 0]) arm.update_gains() return if opts.kill_controller: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[0, 0, 0], p_rot=0, i_trans=[0, 0, 0], i_max_trans=[0, 0, 0], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=0, d_trans=0, p_rot=0, d_rot=0) arm.set_force_directions([0, 0, 0, 0, 0, 0]) arm.set_force([0, 0, 0, 0, 0, 0]) arm.update_gains() return
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 main(): rospy.init_node("switch_controller") from optparse import OptionParser p = OptionParser() p.add_option('-s', '--stiff', dest="stiff", default=False, action="store_true", help="Enable stiff controller.") p.add_option('-f', '--force', dest="force", default=False, action="store_true", help="Enable force controller.") p.add_option('-m', '--force_mag', dest="force_mag", default=2, help="Specify force magnitude.") p.add_option('-x', '--max_force', dest="max_force", default=-1, help="Specify max force magnitude.") p.add_option('-i', '--impedance', dest="impedance", default=False, action="store_true", help="Enable impedance controller.") p.add_option('-c', '--compliance', dest="compliance", default=-1, help="Enable impedance controller.") p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame", help="Set tip to this frame.") p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False, action="store_true", help="Just zero the sensor.") p.add_option('-r', '--reset_pose', dest="reset_pose", default=False, action="store_true", help="Use the saved position in the data file.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(0.1) # reset arm arm.zero_sensor() if opts.zero_sensor: return arm.set_force(6 * [0]) # if opts.reset_pose: pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"]) bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'r') for topic, msg, stamp in bag.read_messages("/teleop_pose"): pose = PoseConverter.to_pos_rot( [msg.position.x, msg.position.y, msg.position.z], [ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ]) for topic, msg, stamp in bag.read_messages("/teleop_posture"): posture = msg.data bag.close() arm.set_posture(posture) i_poses = arm.interpolate_ep(arm.get_end_effector_pose(), pose, 100) for cur_pose in i_poses: arm.set_ep(cur_pose, 1) rospy.sleep(0.1) return # set common parameters arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) if opts.stiff: compliance = float(opts.compliance) if compliance < 0: compliance = 1300 #arm.set_force_gains(p_trans=[1, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_force_gains(p_trans=[1, 0, 0], p_rot=0.1, i_trans=[0.002, 0, 0], i_max_trans=[10, 0, 0], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0) arm.set_force_directions([]) arm.set_force(6 * [0]) elif opts.impedance: compliance = float(opts.compliance) if compliance < 0: compliance = 80 arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0) arm.set_force_directions([]) arm.set_force(6 * [0]) elif opts.force: arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=[float(opts.compliance), 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0) arm.set_force_directions(['x']) arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0]) else: p.print_help() return arm.update_gains()
def main(): rospy.init_node("cool_force_demo") from optparse import OptionParser p = OptionParser() p.add_option('-f', '--force_mag', dest="force_mag", default=2, help="Specify force magnitude.") p.add_option('-x', '--max_force', dest="max_force", default=-1, help="Specify max force magnitude.") p.add_option('-c', '--compliance', dest="compliance", default=-1, help="Compliance to maintain.") p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame", help="Set tip to this frame.") p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False, action="store_true", help="Just zero the sensor.") p.add_option('-l', '--force_line', dest="force_line", default=False, action="store_true", help="Move in a line with zero force.") p.add_option('-p', '--force_plane', dest="force_plane", default=False, action="store_true", help="Move in a plane with zero force.") p.add_option('-o', '--force_point', dest="force_point", default=False, action="store_true", help="Move about a point with zero torque.") p.add_option('-r', '--force_roll', dest="force_roll", default=False, action="store_true", help="Move the wrist with zero torque.") p.add_option('-a', '--force_all', dest="force_all", default=False, action="store_true", help="All DOF are trying to set zero force.") p.add_option('-n', '--force_none', dest="force_none", default=False, action="store_true", help="Return to position control.") p.add_option('-k', '--kill_controller', dest="kill_controller", default=False, action="store_true", help="Render controller dead.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(1) if opts.zero_sensor: arm.zero_sensor() # reset arm arm.set_ep(arm.get_end_effector_pose(), 0) arm.set_force(6 * [0]) fp_trans_major = 3 fp_trans_minor = 1 fp_rot = 0.1 fi_trans_major = 0.002 fi_trans_minor = 0.001 fi_max_trans_major = 10 fi_max_trans_minor = 5 fi_rot = 0 fi_max_rot = 0 pp_trans = 1000 pd_trans_major = 16 pd_trans_minor = 4 pp_rot = 120 pd_rot = 0 if opts.force_line: arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains( p_trans=[fp_trans_major, fp_trans_minor, fp_trans_minor], p_rot=fp_rot, i_trans=[fi_trans_major, fi_trans_minor, fi_trans_minor], i_max_trans=[ fi_max_trans_major, fi_max_trans_minor, fi_max_trans_minor ], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains( p_trans=[float(opts.compliance), pp_trans, pp_trans], d_trans=[pd_trans_major, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions(['x']) arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0]) arm.update_gains() return if opts.force_plane: arm.set_force_max( [-1, float(opts.max_force), float(opts.max_force), -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains( p_trans=[fp_trans_minor, fp_trans_major, fp_trans_major], p_rot=fp_rot, i_trans=[fi_trans_minor, fi_trans_major, fi_trans_major], i_max_trans=[ fi_max_trans_minor, fi_max_trans_major, fi_max_trans_major ], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains( p_trans=[pp_trans, float(opts.compliance), float(opts.compliance)], d_trans=[pd_trans_minor, pd_trans_major, pd_trans_major], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions(['y', 'z']) arm.set_force( [0, float(opts.force_mag), float(opts.force_mag), 0, 0, 0]) arm.update_gains() return if opts.force_point: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains( p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor], p_rot=0.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor], i_max_trans=[ fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor ], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains( p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([0, 0, 0, 1, 1, 1]) arm.set_force([ 0, 0, 0, float(opts.force_mag), float(opts.force_mag), float(opts.force_mag) ]) arm.update_gains() return if opts.force_roll: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains( p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor], p_rot=1.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor], i_max_trans=[ fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor ], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains( p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([0, 0, 0, 1, 0, 0]) arm.set_force([0, 0, 0, float(opts.force_mag), 0, 0]) arm.update_gains() return if opts.force_all: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains( p_trans=6, p_rot=1.8, i_trans=[fi_trans_major, fi_trans_major, fi_trans_major], i_max_trans=[ fi_max_trans_major, fi_max_trans_major, fi_max_trans_major ], i_rot=fi_rot, i_max_rot=fi_max_rot) arm.set_motion_gains( p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_major, pd_trans_major, pd_trans_major], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([1, 1, 1, 1, 1, 1]) arm.set_force([0, 0, 0, 0, 0, 0]) arm.update_gains() return if opts.force_none: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[0, 0, 0], p_rot=0, i_trans=[0, 0, 0], i_max_trans=[0, 0, 0], i_rot=0, i_max_rot=0) arm.set_motion_gains( p_trans=[pp_trans, pp_trans, pp_trans], d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor], p_rot=pp_rot, d_rot=pd_rot) arm.set_force_directions([0, 0, 0, 0, 0, 0]) arm.set_force([0, 0, 0, 0, 0, 0]) arm.update_gains() return if opts.kill_controller: arm.set_force_max([-1, -1, -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) arm.set_force_gains(p_trans=[0, 0, 0], p_rot=0, i_trans=[0, 0, 0], i_max_trans=[0, 0, 0], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=0, d_trans=0, p_rot=0, d_rot=0) arm.set_force_directions([0, 0, 0, 0, 0, 0]) arm.set_force([0, 0, 0, 0, 0, 0]) arm.update_gains() return
import numpy as np import roslib roslib.load_manifest("hrl_pr2_arms") import rospy import rosparam import tf.transformations as tf_trans from hrl_pr2_arms.pr2_arm import create_pr2_arm, PR2ArmJointTrajectory, PR2ArmJTransposeTask from hrl_pr2_arms.pr2_arm_hybrid import PR2ArmHybridForce rospy.init_node("ipython_node") arm = create_pr2_arm('l', PR2ArmHybridForce) # motion gains p - proportional, d - derivative # t - translational, r - rotational kpt, kpr, kdt, kdr = 100, 10, 10, 1 kfpt, kfpr = 1, 0.3 # force gains force_selector = [1, 0, 0, 0, 0, 0] # control force in x direction, motion in others arm.set_motion_gains(kpt, kpr, kdt, kdr) arm.set_force_gains(kfpt, kfpr) arm.update_gains() # force desired is currently at 0 if False: arm.set_force([1, 0, 0, 0, 0, 0]) # apply 1N in x direction (can only command values in x currently)
def main(): rospy.init_node("pr2_arm_test") arm = sys.argv[1] mode = sys.argv[2] assert arm in ['r', 'l'] assert mode in ['joint1', 'joint2', 'cart1', 'cart2', 'cart3', 'cart4'] if mode == 'joint1': pr2_joint_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory) pr2_joint_arm.set_ep([0.1] * 7, 15) if mode == 'joint2': jnt_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory) kin = jnt_arm.kinematics pos = np.mat([[0.6, 0.0, 0.0]]).T rot = np.mat(np.eye(3)) q_ik = kin.IK_search(pos, rot) if q_ik is not None: jnt_arm.set_ep(q_ik, 15) if mode == 'cart1': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jtrans_arm = PR2ArmJTranspose(arm, pr2_r_kinematics) pos = np.mat([[0.6, 0.0, 0.0]]).T rot = np.mat(np.eye(3)) pr2_jtrans_arm.set_ep((pos, rot), 5.) if mode == 'cart2': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics) pr2_jttask_arm.set_gains([0., 60., 60., 50., 50., 50.], [3., 0.1, 0.1, 3., 3., 3.], 'r_gripper_tool_frame') if mode == 'cart3': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics) pos_a = np.mat([[0.0, 0.1, 0.0]]).T rot_a = np.mat(np.eye(3)) pos_b = np.mat([[0.6, 0.0, 0.0]]).T rot_b = np.mat([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]) print zip( *pr2_jttask_arm.interpolate_ep((pos_a, rot_a), (pos_b, rot_b), 10)) if mode == 'cart4': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics) pos_a = np.mat([[0.0, 0.1, 0.0]]).T rot_a = np.mat(tf_trans.random_rotation_matrix())[:3, :3] pos_b = np.mat([[0.6, 0.0, 0.0]]).T rot_b = np.mat(tf_trans.euler_matrix(0.5, 0.8, 0.5))[:3, :3] ep_err = pr2_jttask_arm.ep_error((pos_a, rot_a), (pos_b, rot_b)) err_mat = np.mat( tf_trans.euler_matrix(ep_err[3, 0], ep_err[4, 0], ep_err[5, 0]))[:3, :3] print "err_mat", err_mat diff_mat = rot_b.T * rot_a print "diff_mat", diff_mat rx, ry, rz = tf_trans.euler_from_matrix(diff_mat) print ep_err[3:6, 0].T print rx, ry, rz print diff_mat - err_mat
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("pr2_arm_test") arm = sys.argv[1] mode = sys.argv[2] assert arm in ['r', 'l'] assert mode in ['joint1', 'joint2', 'cart1', 'cart2', 'cart3', 'cart4'] if mode == 'joint1': pr2_joint_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory) pr2_joint_arm.set_ep([0.1]*7, 15) if mode == 'joint2': jnt_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory) kin = jnt_arm.kinematics pos = np.mat([[0.6, 0.0, 0.0]]).T rot = np.mat(np.eye(3)) q_ik = kin.IK_search(pos, rot) if q_ik is not None: jnt_arm.set_ep(q_ik, 15) if mode == 'cart1': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jtrans_arm = PR2ArmJTranspose(arm, pr2_r_kinematics) pos = np.mat([[0.6, 0.0, 0.0]]).T rot = np.mat(np.eye(3)) pr2_jtrans_arm.set_ep((pos, rot), 5.) if mode == 'cart2': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics) pr2_jttask_arm.set_gains([0., 60., 60., 50., 50., 50.], [3., 0.1, 0.1, 3., 3., 3.], 'r_gripper_tool_frame') if mode == 'cart3': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics) pos_a = np.mat([[0.0, 0.1, 0.0]]).T rot_a = np.mat(np.eye(3)) pos_b = np.mat([[0.6, 0.0, 0.0]]).T rot_b = np.mat([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]) print zip(*pr2_jttask_arm.interpolate_ep((pos_a, rot_a), (pos_b, rot_b), 10)) if mode == 'cart4': pr2_r_kinematics = PR2ArmKinematics(arm) pr2_jttask_arm = PR2ArmJTransposeTask(arm, pr2_r_kinematics) pos_a = np.mat([[0.0, 0.1, 0.0]]).T rot_a = np.mat(tf_trans.random_rotation_matrix())[:3,:3] pos_b = np.mat([[0.6, 0.0, 0.0]]).T rot_b = np.mat(tf_trans.euler_matrix(0.5, 0.8, 0.5))[:3,:3] ep_err = pr2_jttask_arm.ep_error((pos_a, rot_a), (pos_b, rot_b)) err_mat = np.mat(tf_trans.euler_matrix(ep_err[3,0], ep_err[4,0], ep_err[5,0]))[:3, :3] print "err_mat", err_mat diff_mat = rot_b.T * rot_a print "diff_mat", diff_mat rx, ry, rz = tf_trans.euler_from_matrix(diff_mat) print ep_err[3:6,0].T print rx, ry, rz print diff_mat - err_mat
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): 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("switch_controller") from optparse import OptionParser p = OptionParser() p.add_option('-s', '--stiff', dest="stiff", default=False, action="store_true", help="Enable stiff controller.") p.add_option('-f', '--force', dest="force", default=False, action="store_true", help="Enable force controller.") p.add_option('-m', '--force_mag', dest="force_mag", default=2, help="Specify force magnitude.") p.add_option('-x', '--max_force', dest="max_force", default=-1, help="Specify max force magnitude.") p.add_option('-i', '--impedance', dest="impedance", default=False, action="store_true", help="Enable impedance controller.") p.add_option('-c', '--compliance', dest="compliance", default=-1, help="Enable impedance controller.") p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame", help="Set tip to this frame.") p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False, action="store_true", help="Just zero the sensor.") p.add_option('-r', '--reset_pose', dest="reset_pose", default=False, action="store_true", help="Use the saved position in the data file.") (opts, args) = p.parse_args() arm = create_pr2_arm('l', PR2ArmHybridForce) rospy.sleep(0.1) # reset arm arm.zero_sensor() if opts.zero_sensor: return arm.set_force(6 * [0]) # if opts.reset_pose: pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"]) bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'r') for topic, msg, stamp in bag.read_messages("/teleop_pose"): pose = PoseConverter.to_pos_rot([msg.position.x, msg.position.y, msg.position.z], [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]) for topic, msg, stamp in bag.read_messages("/teleop_posture"): posture = msg.data bag.close() arm.set_posture(posture) i_poses = arm.interpolate_ep(arm.get_end_effector_pose(), pose, 100) for cur_pose in i_poses: arm.set_ep(cur_pose, 1) rospy.sleep(0.1) return # set common parameters arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1]) arm.set_tip_frame(opts.tip_frame) if opts.stiff: compliance = float(opts.compliance) if compliance < 0: compliance = 1300 #arm.set_force_gains(p_trans=[1, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_force_gains(p_trans=[1, 0, 0], p_rot=0.1, i_trans=[0.002, 0, 0], i_max_trans=[10, 0, 0], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0) arm.set_force_directions([]) arm.set_force(6 * [0]) elif opts.impedance: compliance = float(opts.compliance) if compliance < 0: compliance = 80 arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0) arm.set_force_directions([]) arm.set_force(6 * [0]) elif opts.force: arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0) arm.set_motion_gains(p_trans=[float(opts.compliance), 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0) arm.set_force_directions(['x']) arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0]) else: p.print_help() return arm.update_gains()
#! /usr/bin/python import numpy as np import roslib roslib.load_manifest("hrl_pr2_arms") import rospy import tf import tf.transformations as tf_trans from std_msgs.msg import Bool, Float32 from std_srvs.srv import Empty from geometry_msgs.msg import PoseStamped, Vector3 from hrl_generic_arms.pose_converter import PoseConverter from hrl_pr2_arms.pr2_arm import create_pr2_arm from hrl_pr2_arms.pr2_arm_hybrid import PR2ArmHybridForce rospy.init_node("test_controller") arm = create_pr2_arm("l", PR2ArmHybridForce) arm.use_auto_update(True) rospy.sleep(0.1) arm.zero_sensor() arm.reset_ep() t, R = arm.get_ep() arm.set_ep((t, R), 1) q = arm.get_joint_angles() arm.set_posture(q.tolist()[0:3] + 4 * [9999])