def main(): rospy.init_node('test_pid_joint_control') pid_jc_factory = PIDJCFactory('r', [0.05] * 7, [0.25] * 7, [0.04] * 7, [0.02] * 7, [0.070] * 7, [0.4] * 7, 10., 0.1) q = [ -0.2795571923841168, 0.9737173354972255, 0.60073767302223002, -1.445674103265479, -6.0812198392665415, -1.1110966461101544, 0.3056365620075685 ] q = [-0.1, 0.0, 0.0, -0.5, 0.0, -0.4, 0.0] pid_jc = pid_jc_factory.create_pid_jc(q) epc = EPC('test_pid_joint_control') epc.epc_motion(pid_jc, 0.1, 50)
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_epc') pg.init() screen = pg.display.set_mode((300,300)) test_gen = TestEPGenerator(100) test_epc = EPC('test_epc') # wait for keys p for pause and s for stop test_epc.last_time = rospy.get_time() def keyboard_cb(timer_event): pg.event.get() keys = pg.key.get_pressed() if keys[pg.K_s] or keys[pg.K_q]: test_epc.stop_epc = True if keys[pg.K_p] and rospy.get_time() - test_epc.last_time > 0.3: test_epc.pause_epc = not test_epc.pause_epc test_epc.last_time = rospy.get_time() Timer(rospy.Duration(0.1), keyboard_cb) test_epc.epc_motion(test_gen, 0.1, 10)