Beispiel #1
0
def main():
    N = int(DURATION / DT) + 1

    model = models.ThreeInputModel(output_idx=[0, 1])

    ts = DT * np.arange(N)
    q0 = np.array([0, np.pi / 4.0, -np.pi / 4.0])
    p0 = model.forward(q0)

    sim = PymunkSimulationTorque(DT, iterations=10)
    sim.add_robot(model, q0)

    box_body = pymunk.Body()
    box_body.position = (p0[0], p0[1] + 0.1)
    box_corners = [(-0.2, 0.05), (-0.2, -0.05), (0.2, -0.05), (0.2, 0.05)]
    box = pymunk.Poly(box_body, box_corners, radius=0.01)
    box.mass = 0.5
    box.friction = 0.75
    # sim.space.add(box.body, box)

    n_balls = 20
    ball_r = 0.1
    ball_x = np.random.random(n_balls) * 8 - 2
    ball_y = np.random.random(n_balls) * 3 + 2
    for i in range(n_balls):
        body = pymunk.Body()
        body.position = (ball_x[i], ball_y[i])
        ball = pymunk.Circle(body, ball_r, (0, 0))
        ball.mass = 0.1
        ball.color = (255, 0, 0, 255)
        ball.friction = 0.5
        sim.space.add(body, ball)

    W = 0.01 * np.eye(model.ni)
    K = np.eye(model.no)
    controller = control.DiffIKController(model, W, K, DT, model.vel_lim,
                                          model.acc_lim)

    # timescaling = trajectories.QuinticTimeScaling(DURATION)
    # trajectory = trajectories.Sine(p0, 2, 0.5, 1, timescaling, DURATION)
    trajectory = trajectories.Point(p0)

    ps = np.zeros((N, model.no))
    pds = np.zeros((N, model.no))

    robot_renderer = plotter.ThreeInputRenderer(model, q0)
    box_renderer = plotter.PolygonRenderer(np.array(box.body.position),
                                           box.body.angle,
                                           np.array(box_corners))
    # trajectory_renderer = plotter.TrajectoryRenderer(trajectory, ts)
    plot = plotter.RealtimePlotter([robot_renderer, box_renderer])
    plot.start(grid=True)

    plt.ion()
    fig = plt.figure()
    ax = plt.gca()

    # video = plotter.Video(name='balls.mp4', fps=1./(PLOT_PERIOD * DT))
    # video.setup(fig)

    ax.set_xlabel('x (m)')
    ax.set_ylabel('y (m)')
    ax.set_xlim([-1, 6])
    ax.set_ylim([-1, 2])

    ax.set_aspect('equal')

    options = pymunk.matplotlib_util.DrawOptions(ax)
    options.flags = pymunk.SpaceDebugDrawOptions.DRAW_SHAPES

    sim.space.debug_draw(options)
    fig.canvas.draw()
    fig.canvas.flush_events()

    # video.writer.grab_frame()

    q = q0
    dq = np.zeros(3)
    u = np.zeros(3)
    pd, vd, ad = trajectory.sample(0, flatten=True)

    ps[0, :] = p0
    pds[0, :] = pd[:model.no]

    kp = 5
    kd = 3
    ki = 0.1
    ddqd = np.zeros(3)
    dqd = np.zeros(3)
    qd = q0

    e_sum = 0

    for i in range(N - 1):
        t = ts[i]

        # controller
        if i % CTRL_PERIOD == 0:
            pd, vd, ad = trajectory.sample(t, flatten=True)
            u = controller.solve(q, dq, pd, vd)
            # sim.command_velocity(u)

            # torque control law
            e = qd - q
            e_sum += CTRL_PERIOD * DT * e
            α = ddqd + kp * e + kd * (dqd - dq) + ki * e_sum
            tau = model.calc_torque(q, dq, α)
            sim.command_torque(tau)

        # step the sim
        q, dq = sim.step()

        p = model.forward(q)
        ps[i + 1, :] = p
        pds[i + 1, :] = pd[:model.no]

        if i % PLOT_PERIOD == 0:
            box_renderer.set_state(np.array(box.body.position), box.body.angle)
            robot_renderer.set_state(q)

            ax.cla()
            ax.set_xlim([-1, 6])
            ax.set_ylim([-1, 2])

            sim.space.debug_draw(options)
            fig.canvas.draw()
            fig.canvas.flush_events()
            # video.writer.grab_frame()

            plot.update()
    plot.done()
    # video.writer.finish()

    xe = pds[1:, 0] - ps[1:, 0]
    ye = pds[1:, 1] - ps[1:, 1]
    print('RMSE(x) = {}'.format(rms(xe)))
    print('RMSE(y) = {}'.format(rms(ye)))
def main():
    N = int(DURATION / DT) + 1

    model = TopDownHolonomicModel(L1, L2, VEL_LIM, acc_lim=ACC_LIM, output_idx=[0, 1])

    Q = np.eye(model.no)
    R = np.eye(model.ni) * 0.1
    controller = control.ObstacleAvoidingMPC(model, MPC_DT, Q, R, VEL_LIM, ACC_LIM)

    ts = DT * np.arange(N)
    qs = np.zeros((N, model.ni))
    dqs = np.zeros((N, model.ni))
    us = np.zeros((N, model.ni))
    ps = np.zeros((N, model.no))
    vs = np.zeros((N, model.no))
    pds = np.zeros((N, model.no))

    # initial state
    q = np.array([0, 0, 0, 0.25*np.pi, -0.5*np.pi])
    p = model.forward(q)
    dq = np.zeros(model.ni)

    # obstacle
    pc = np.array([3., 0])
    obs = obstacle.Circle(0.5, 10)

    pg = np.array([5., 0])

    qs[0, :] = q
    ps[0, :] = p
    pds[0, :] = p

    circle_renderer = plotter.CircleRenderer(obs.r, pc)
    robot_renderer = plotter.TopDownHolonomicRenderer(model, q)
    plot = plotter.RealtimePlotter([robot_renderer, circle_renderer])
    plot.start(limits=[-5, 10, -5, 10], grid=True)

    for i in range(N - 1):
        n = min(NUM_HORIZON, N - 1 - i)
        pd = np.tile(pg, n)
        u = controller.solve(q, dq, pd, n, pc)

        # step the model
        q, dq = model.step(q, u, DT, dq_last=dq)
        p = model.forward(q)
        v = model.jacobian(q).dot(dq)

        # obstacle interaction
        f, movement = obs.calc_point_force(pc, p)
        pc += movement

        # record
        us[i, :] = u
        dqs[i+1, :] = dq
        qs[i+1, :] = q
        ps[i+1, :] = p
        pds[i+1, :] = pd[:model.no]
        vs[i+1, :] = v

        # render
        robot_renderer.set_state(q)
        circle_renderer.set_state(pc)
        plot.update()
    plot.done()

    xe = pds[1:, 0] - ps[1:, 0]
    ye = pds[1:, 1] - ps[1:, 1]
    print('RMSE(x) = {}'.format(rms(xe)))
    print('RMSE(y) = {}'.format(rms(ye)))

    plt.figure()
    plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
    plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
    plt.plot(ts, ps[:, 0],  label='$x$', color='b')
    plt.plot(ts, ps[:, 1],  label='$y$', color='r')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Position')
    plt.title('End effector position')

    plt.figure()
    plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
    plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
    plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
    plt.grid()
    plt.legend()
    plt.title('Actual joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, us[:, 0], label='$u_x$')
    plt.plot(ts, us[:, 1], label='$u_1$')
    plt.plot(ts, us[:, 2], label='$u_2$')
    plt.grid()
    plt.legend()
    plt.title('Commanded joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, qs[:, 0], label='$q_x$')
    plt.plot(ts, qs[:, 1], label='$q_1$')
    plt.plot(ts, qs[:, 2], label='$q_2$')
    plt.grid()
    plt.legend()
    plt.title('Joint positions')
    plt.xlabel('Time (s)')
    plt.ylabel('Joint positions')

    plt.show()
Beispiel #3
0
def main():
    N = int(DURATION / DT) + 1

    model = models.ThreeInputModel(l1=L1,
                                   l2=L2,
                                   vel_lim=VEL_LIM,
                                   acc_lim=ACC_LIM,
                                   output_idx=[0, 1])

    W = 0.1 * np.eye(model.ni)
    K = np.eye(model.no)
    controller = control.DiffIKController(model, W, K, DT, VEL_LIM, ACC_LIM)

    ts = DT * np.arange(N)
    qs = np.zeros((N, model.ni))
    dqs = np.zeros((N, model.ni))
    us = np.zeros((N, model.ni))
    ps = np.zeros((N, model.no))
    vs = np.zeros((N, model.no))
    pds = np.zeros((N, model.no))

    q0 = np.array([0, np.pi / 4.0, -np.pi / 4.0])
    p0 = model.forward(q0)

    # reference trajectory
    # trajectory = Line(p0, v0=np.zeros(2), a=np.array([0.01, 0]))
    # timescaling = trajectories.QuinticTimeScaling(DURATION)
    timescaling = trajectories.TrapezoidalTimeScalingV(0.15, DURATION)
    # timescaling = trajectories.TrapezoidalTimeScalingA(0.1, DURATION)

    # trajectory = trajectories.PointToPoint(p0, p0 + [1, 0], timescaling, DURATION)
    trajectory = trajectories.Sine(p0, 2, 0.5, 1, timescaling, DURATION)
    # trajectory2 = PointToPoint(p0 + [1, 0], p0 + [2, 0], timescaling, 0.5*DURATION)
    # trajectory = Chain([trajectory1, trajectory2])

    # points = np.array([p0, p0 + [1, 1], p0 + [2, -1], p0 + [3, 0]])
    # trajectory = CubicBezier(points, timescaling, DURATION)
    # trajectory = trajectories.Circle(p0, 0.5, timescaling, DURATION)
    # points = np.array([p0, p0 + [1, 0], p0 + [1, -1], p0 + [0, -1], p0])
    # trajectory = Polygon(points, v=0.4)

    # pref, vref, aref = trajectory.sample(ts)
    # plt.figure()
    # plt.plot(ts, pref[:, 0], label='$p$')
    # plt.plot(ts, vref[:, 0], label='$v$')
    # plt.plot(ts, aref[:, 0], label='$a$')
    # plt.grid()
    # plt.legend()
    # plt.title('EE reference trajectory')
    # plt.xlabel('Time (s)')
    # plt.ylabel('Reference signal')
    # plt.show()

    q = q0
    p = p0
    dq = np.zeros(model.ni)
    qs[0, :] = q0
    ps[0, :] = p0
    pds[0, :] = p0

    robot_renderer = plotter.ThreeInputRenderer(model, q0)
    trajectory_renderer = plotter.TrajectoryRenderer(trajectory, ts)
    plot = plotter.RealtimePlotter([robot_renderer, trajectory_renderer])
    plot.start(grid=True)

    for i in range(N - 1):
        t = ts[i]

        # controller
        pd, vd, ad = trajectory.sample(t, flatten=True)
        u = controller.solve(q, dq, pd, vd)

        # step the model
        q, dq = model.step(q, u, DT, dq_last=dq)
        p = model.forward(q)
        v = model.jacobian(q).dot(dq)

        # record
        us[i, :] = u
        dqs[i + 1, :] = dq
        qs[i + 1, :] = q
        ps[i + 1, :] = p
        pds[i + 1, :] = pd[:model.no]
        vs[i + 1, :] = v

        # render
        robot_renderer.set_state(q)
        plot.update()
    plot.done()

    xe = pds[1:, 0] - ps[1:, 0]
    ye = pds[1:, 1] - ps[1:, 1]
    print('RMSE(x) = {}'.format(rms(xe)))
    print('RMSE(y) = {}'.format(rms(ye)))

    plt.figure()
    plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
    plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
    plt.plot(ts, ps[:, 0], label='$x$', color='b')
    plt.plot(ts, ps[:, 1], label='$y$', color='r')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Position')
    plt.title('End effector position')

    plt.figure()
    plt.plot(ts, vs[:, 0], label='$v_x$', color='b')
    plt.plot(ts, vs[:, 1], label='$v_y$', color='r')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (m/s)')
    plt.title('End effector velocity')

    plt.figure()
    plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
    plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
    plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
    plt.grid()
    plt.legend()
    plt.title('Actual joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, us[:, 0], label='$u_x$')
    plt.plot(ts, us[:, 1], label='$u_1$')
    plt.plot(ts, us[:, 2], label='$u_2$')
    plt.grid()
    plt.legend()
    plt.title('Commanded joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, qs[:, 0], label='$q_x$')
    plt.plot(ts, qs[:, 1], label='$q_1$')
    plt.plot(ts, qs[:, 2], label='$q_2$')
    plt.grid()
    plt.legend()
    plt.title('Joint positions')
    plt.xlabel('Time (s)')
    plt.ylabel('Joint positions')

    plt.show()
Beispiel #4
0
def main():
    N = int(DURATION / DT) + 1

    model = ThreeInputModel(L1, L2, VEL_LIM, acc_lim=ACC_LIM, output_idx=[0, 1])

    Q = np.eye(model.no)
    R = np.eye(model.ni) * 0.01
    mpc = MPC(model, DT, Q, R, VEL_LIM, ACC_LIM)

    ts = DT * np.arange(N)
    qs = np.zeros((N, model.ni))
    dqs = np.zeros((N, model.ni))
    us = np.zeros((N, model.ni))
    ps = np.zeros((N, model.no))
    vs = np.zeros((N, model.no))
    pds = np.zeros((N, model.no))

    q0 = np.array([0, np.pi/4.0, -np.pi/4.0])
    p0 = model.forward(q0)

    # reference trajectory
    timescaling = QuinticTimeScaling(DURATION)
    points = np.array([p0, p0 + [1, 1], p0 + [2, -1], p0 + [3, 0]])
    trajectory = CubicBezier(points, timescaling, DURATION)

    # obstacles
    # obs = Wall(x=2.5)
    # obs = Circle(c=np.array([3.0, 1.5]), r=1)

    q = q0
    p = p0
    dq = np.zeros(model.ni)
    qs[0, :] = q0
    ps[0, :] = p0
    pds[0, :] = p0

    robot_renderer = ThreeInputRenderer(model, q0)
    trajectory_renderer = TrajectoryRenderer(trajectory, ts)
    plotter = RealtimePlotter([robot_renderer, trajectory_renderer])
    plotter.start()

    for i in range(N - 1):
        # MPC
        # The +1 ts[i+1] is because we want to generate a u[i] such that
        # p[i+1] = FK(q[i+1]) = pd[i+1]
        n = min(NUM_HORIZON, N - 1 - i)
        pd, _, _ = trajectory.sample(ts[i+1:i+1+n], flatten=True)
        u = mpc.solve(q, dq, pd, n)

        q, dq = model.step(q, u, DT, dq_last=dq)
        p = model.forward(q)
        v = model.jacobian(q).dot(dq)

        # record
        us[i, :] = u

        dqs[i+1, :] = dq
        qs[i+1, :] = q
        ps[i+1, :] = p
        pds[i+1, :] = pd[:model.no]
        vs[i+1, :] = v

        robot_renderer.set_state(q)
        plotter.update()
    plotter.done()

    xe = pds[1:, 0] - ps[1:, 0]
    ye = pds[1:, 1] - ps[1:, 1]
    print('RMSE(x) = {}'.format(rms(xe)))
    print('RMSE(y) = {}'.format(rms(ye)))

    plt.figure()
    plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
    plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
    plt.plot(ts, ps[:, 0],  label='$x$', color='b')
    plt.plot(ts, ps[:, 1],  label='$y$', color='r')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Position')
    plt.title('End effector position')

    plt.figure()
    plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
    plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
    plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
    plt.grid()
    plt.legend()
    plt.title('Actual joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, us[:, 0], label='$u_x$')
    plt.plot(ts, us[:, 1], label='$u_1$')
    plt.plot(ts, us[:, 2], label='$u_2$')
    plt.grid()
    plt.legend()
    plt.title('Commanded joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.show()
Beispiel #5
0
def main():
    N = int(DURATION / DT) + 1

    model = ThreeInputModel(L1, L2, VEL_LIM, acc_lim=ACC_LIM, output_idx=[0, 1])

    W = 0.1 * np.eye(model.ni)
    Kp = np.eye(model.no)
    Kv = 0.1 * np.eye(model.no)
    controller = AccelerationController(model, W, Kp, Kv, DT, VEL_LIM, ACC_LIM)

    ts = np.array([i * DT for i in range(N)])
    qs = np.zeros((N, model.ni))
    dqs = np.zeros((N, model.ni))
    us = np.zeros((N, model.ni))
    ps = np.zeros((N, model.no))
    vs = np.zeros((N, model.no))
    pds = np.zeros((N, model.no))
    dq_cmds = np.zeros((N, model.ni))

    q0 = np.array([0, np.pi/4.0, -np.pi/4.0])
    p0 = model.forward(q0)

    # reference trajectory
    timescaling = QuinticTimeScaling(DURATION)
    trajectory = PointToPoint(p0, p0 + [1, 0], timescaling, DURATION)

    # pref, vref, aref = trajectory.sample(ts)
    # plt.figure()
    # plt.plot(ts, pref[:, 0], label='$p$')
    # plt.plot(ts, vref[:, 0], label='$v$')
    # plt.plot(ts, aref[:, 0], label='$a$')
    # plt.grid()
    # plt.legend()
    # plt.title('EE reference trajectory')
    # plt.xlabel('Time (s)')
    # plt.ylabel('Reference signal')
    # plt.show()

    q = q0
    p = p0
    dq = np.zeros(model.ni)
    qs[0, :] = q0
    ps[0, :] = p0
    pds[0, :] = p0

    robot_renderer = ThreeInputRenderer(model, q0)
    trajectory_renderer = TrajectoryRenderer(trajectory, ts)
    plotter = RealtimePlotter([robot_renderer, trajectory_renderer])
    plotter.start()

    for i in range(N - 1):

        t = ts[i]

        # controller
        pd, vd, ad = trajectory.sample(t, flatten=True)
        u = controller.solve(q, dq, pd, vd, ad)
        dq_cmd = dq + DT * u

        # step the model
        q, dq = model.step(q, dq_cmd, DT, dq_last=dq)
        p = model.forward(q)
        v = model.jacobian(q).dot(dq)

        # record
        us[i, :] = u
        dq_cmds[i, :] = dq_cmd

        dqs[i+1, :] = dq
        qs[i+1, :] = q
        ps[i+1, :] = p
        pds[i+1, :] = pd[:model.no]
        vs[i+1, :] = v

        robot_renderer.set_state(q)
        plotter.update()
    plotter.done()

    xe = pds[1:, 0] - ps[1:, 0]
    ye = pds[1:, 1] - ps[1:, 1]
    print('RMSE(x) = {}'.format(rms(xe)))
    print('RMSE(y) = {}'.format(rms(ye)))

    plt.figure()
    plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
    plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
    plt.plot(ts, ps[:, 0],  label='$x$', color='b')
    plt.plot(ts, ps[:, 1],  label='$y$', color='r')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Position')
    plt.title('End effector position')

    plt.figure()
    plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
    plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
    plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
    plt.grid()
    plt.legend()
    plt.title('Joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, dq_cmds[:, 0], label='$\\dot{q}_x$')
    plt.plot(ts, dq_cmds[:, 1], label='$\\dot{q}_1$')
    plt.plot(ts, dq_cmds[:, 2], label='$\\dot{q}_2$')
    plt.grid()
    plt.legend()
    plt.title('Commanded Joint Velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, us[:, 0], label='$u_x$')
    plt.plot(ts, us[:, 1], label='$u_1$')
    plt.plot(ts, us[:, 2], label='$u_2$')
    plt.grid()
    plt.legend()
    plt.title('Commanded joint acceleration')
    plt.xlabel('Time (s)')
    plt.ylabel('Acceleration')

    plt.figure()
    plt.plot(ts, qs[:, 0], label='$q_x$')
    plt.plot(ts, qs[:, 1], label='$q_1$')
    plt.plot(ts, qs[:, 2], label='$q_2$')
    plt.grid()
    plt.legend()
    plt.title('Joint positions')
    plt.xlabel('Time (s)')
    plt.ylabel('Joint positions')

    plt.show()
Beispiel #6
0
def main():
    N = int(DURATION / DT) + 1

    model = models.TopDownHolonomicModel(L1,
                                         L2,
                                         VEL_LIM,
                                         acc_lim=ACC_LIM,
                                         output_idx=[0, 1])

    W = 0.1 * np.eye(model.ni)
    K = np.eye(model.no)
    controller = control.ConstrainedDiffIKController(model, W, K, DT, VEL_LIM,
                                                     ACC_LIM)

    ts = DT * np.arange(N)
    qs = np.zeros((N, model.ni))
    dqs = np.zeros((N, model.ni))
    us = np.zeros((N, model.ni))
    ps = np.zeros((N, model.no))
    vs = np.zeros((N, model.no))
    pds = np.zeros((N, model.no))

    # initial state
    q = np.array([0, 0, 0, 0.25 * np.pi, -0.5 * np.pi])
    p = model.forward(q)
    dq = np.zeros(model.ni)
    f = np.zeros(2)

    # obstacle
    pc = np.array([-2., 1.])
    obs = obstacle.Circle(0.5, 1000)

    # goal position
    pg = np.array([5., 0])

    qs[0, :] = q
    ps[0, :] = p
    pds[0, :] = p

    circle_renderer = plotter.CircleRenderer(obs.r, pc)
    robot_renderer = plotter.TopDownHolonomicRenderer(model, q)
    plot = plotter.RealtimePlotter([robot_renderer, circle_renderer])
    plot.start(limits=[-5, 10, -5, 10], grid=True)

    for i in range(N - 1):
        # experimental controller for aligning and pushing object to a goal
        # point - generates a desired set point pd; the admittance portion
        # doesn't really seem helpful at this point (since we actually *want*
        # to hit/interact with the environment)
        b = 0.25
        cos_alpha = np.cos(np.pi * 0.25)
        p1 = pc - 0.25 * unit(pg - pc)

        d = np.linalg.norm(p - pc)
        J = model.jacobian(q)

        # distance from obstacle constraints
        A = DT * (p - pc).T.dot(J) / d
        A = A.reshape((1, model.ni))
        lbA = np.array([obs.r + b - d])

        # TODO velocity damper constraints - these are not working correctly
        # d = d - obs.r
        # xi = 1
        # ds = 0.1
        # di = 1
        # no = unit(pc - p)
        # A = no.dot(J)
        # A = A.reshape((model.ni, 1))
        # lbA = np.array([-xi * (d - ds) / (di - ds)])
        #
        # if d >= di:
        #     print(d)
        #     lbA = np.zeros_like(lbA)
        #     A = np.zeros_like(A)

        # only enforced away from the push location
        # TODO other option is to revisit the cardioid stuff, which has the
        # advantage of not being discontinous in distance
        cos_angle = unit(p - pc).dot(unit(p1 - pc))
        if cos_angle >= cos_alpha:
            lbA = None
            A = None

        vd = np.zeros(2)
        pd = p1

        u = controller.solve(q, dq, pd, vd, A=A, lbA=lbA, ubA=None)

        # step the model
        q, dq = model.step(q, u, DT, dq_last=dq)
        p = model.forward(q)
        v = model.jacobian(q).dot(dq)

        # obstacle interaction
        f = obs.calc_point_force(pc, p)
        f, movement = obs.apply_force(f)
        pc += movement

        # if object is close enough to the goal position, stop
        if np.linalg.norm(pg - pc) < 0.1:
            print('done')
            break

        # record
        us[i, :] = u
        dqs[i + 1, :] = dq
        qs[i + 1, :] = q
        ps[i + 1, :] = p
        pds[i + 1, :] = pd[:model.no]
        vs[i + 1, :] = v

        # render
        robot_renderer.set_state(q)
        circle_renderer.set_state(pc)
        plot.update()
    plot.done()

    xe = pds[1:, 0] - ps[1:, 0]
    ye = pds[1:, 1] - ps[1:, 1]
    print('RMSE(x) = {}'.format(rms(xe)))
    print('RMSE(y) = {}'.format(rms(ye)))

    plt.figure()
    plt.plot(ts, pds[:, 0], label='$x_d$', color='b', linestyle='--')
    plt.plot(ts, pds[:, 1], label='$y_d$', color='r', linestyle='--')
    plt.plot(ts, ps[:, 0], label='$x$', color='b')
    plt.plot(ts, ps[:, 1], label='$y$', color='r')
    plt.grid()
    plt.legend()
    plt.xlabel('Time (s)')
    plt.ylabel('Position')
    plt.title('End effector position')

    plt.figure()
    plt.plot(ts, dqs[:, 0], label='$\\dot{q}_x$')
    plt.plot(ts, dqs[:, 1], label='$\\dot{q}_1$')
    plt.plot(ts, dqs[:, 2], label='$\\dot{q}_2$')
    plt.grid()
    plt.legend()
    plt.title('Actual joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, us[:, 0], label='$u_x$')
    plt.plot(ts, us[:, 1], label='$u_1$')
    plt.plot(ts, us[:, 2], label='$u_2$')
    plt.grid()
    plt.legend()
    plt.title('Commanded joint velocity')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity')

    plt.figure()
    plt.plot(ts, qs[:, 0], label='$q_x$')
    plt.plot(ts, qs[:, 1], label='$q_1$')
    plt.plot(ts, qs[:, 2], label='$q_2$')
    plt.grid()
    plt.legend()
    plt.title('Joint positions')
    plt.xlabel('Time (s)')
    plt.ylabel('Joint positions')

    plt.show()