q_des_log = np.vstack((q_des_log, q_des))
    qd_log = np.vstack((qd_log, qd))
    qd_des_log = np.vstack((qd_des_log, qd_des))
    qdd_log = np.vstack((qdd_log, qdd))
    qdd_des_log = np.vstack((qdd_des_log, qdd_des))
    tau_log = np.vstack((tau_log, tau))
    f_log = np.vstack((f_log, f))
    x_log = np.vstack((x_log, x))

    # update time
    time = time + conf.dt

    # plot contact force
    # scale f for plotting purposes
    scaled_f = f / 100
    ros_pub.add_arrow(x, scaled_f)

    # plot ball at the end-effector
    ros_pub.add_marker(x)
    ros_pub.publish(robot, q, qd, tau)
    tm.sleep(conf.dt * conf.SLOW_FACTOR)
    # stops the while loop if  you prematurely hit CTRL+C
    if ros_pub.isShuttingDown():
        print("Shutting Down")
        break

ros_pub.deregister_node()

# plot joint variables
plotJoint('position', 0, time_log, q_log, q_des_log, qd_log, qd_des_log,
          qdd_log, qdd_des_log, tau_log)
    #EXERSISE 3.6 : full task space inverse dynamics (computed torque)
    # compute lambda for both orientation and position
    rho = 0.00001  # damping factor
    #    print "J6 rank:", np.linalg.matrix_rank(J6)
    #    lambda6_= np.linalg.inv(J6.dot(M_inv).dot( J6.T))  #singular
    #    lambda6_ = np.linalg.inv(J6.dot(M_inv).dot( J6.T) + pow(rho,2)*eye(6)) # damped inertia matrix
    #    #J6Tpinv = np.linalg.pinv(J6.T, rho) # damped pseudoinverse using native function
    #    J6Tpinv = np.linalg.inv(J6.dot(J6.T) + pow(rho,2)*eye(6)).dot(J6)  # damped pseudoinverse explicitely computed
    #    Jdqd6 = robot.frameClassicAcceleration(q, qd, None, frame_ee).vector
    #    mu6 =  J6Tpinv.dot(h) -lambda6_.dot(Jdqd6)
    #    tau = J6.T.dot(lambda6_.dot(np.hstack((pdd_des + F_des, omega_d_des + Gamma_des))) + mu6)

    #    EXERCISE 2.3: Add an external force
    if conf.EXTERNAL_FORCE and time > 1.0:
        tau += J.transpose().dot(conf.extForce)
        ros_pub.add_arrow(p, conf.extForce / 100)

    #SIMULATION of the forward dynamics
    qdd = M_inv.dot(tau - h)

    # Forward Euler Integration
    qd = qd + qdd * conf.dt
    q = q + qd * conf.dt + 0.5 * conf.dt * conf.dt * qdd

    # Log Data into a vector
    time_log = np.append(time_log, time)
    p_log = np.vstack((p_log, p))
    p_des_log = np.vstack((p_des_log, p_des))
    pd_log = np.vstack((pd_log, pd))
    pd_des_log = np.vstack((pd_des_log, pd_des))
    pdd_des_log = np.vstack((pdd_des_log, pdd_des))
        #            # X component
        ##            if (F_env[0] >= conf.mu * F_env[2]):
        ##                F_env[0] = conf.mu * F_env[2]
        ##            if (F_env[0] <= -conf.mu * F_env[2]):
        ##                F_env[0] = -conf.mu * F_env[2]
        ##            # Y component
        ##            if (F_env[1] >= conf.mu * F_env[2]):
        ##                F_env[1] = conf.mu * F_env[2]
        ##            if (F_env[1] <= -conf.mu * F_env[2]):
        ##                F_env[1] = -conf.mu * F_env[2]
        ##            ros_pub.add_cone(p, conf.n, conf.mu, color = "blue")
        #        else:
        #            contact_sampled = False
        #            F_env = np.array([0.0, 0.0, 0.0])

        ros_pub.add_arrow(p, F_env / 100)
        tau += J.transpose().dot(F_env)
    ros_pub.add_marker(p)

    #SIMULATION of the forward dynamics
    M_inv = np.linalg.inv(M)
    qdd = M_inv.dot(tau - h)

    # Forward Euler Integration
    qd = qd + qdd * conf.dt
    q = q + conf.dt * qd + 0.5 * conf.dt * conf.dt * qdd

    # Log Data into a vector
    time_log = np.append(time_log, time)
    q_log = np.vstack((q_log, q))
    q_des_log = np.vstack((q_des_log, q_des))