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)
