need to fix something broken in p path path following, but something is happening

python/examples/crocoddyl_ocp_clik.py

@@ -29,6 +29,8 @@ if __name__ == "__main__":
     # both because there can be disturbances,
     # and because it is sampled at a much lower frequency
     jointTrajFollowingPD(args, robot, reference)
+    #reference.pop('dt')
+    #plotFromDict(reference, args.n_knots + 1, args)
 
     print("final position:")
     print(robot.getT_w_e())

python/examples/heron_pls.py

+import numpy as np
+import argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
+from ur_simple_control.util.get_model import heron_approximation 
+import time
+from pinocchio.visualize import MeshcatVisualizer
+import pinocchio as pin
+
+def get_args():
+    parser = getMinimalArgParser()
+    parser.description = 'mpc for heron path following'
+    parser = get_OCP_args(parser)
+    args = parser.parse_args()
+    return args
+
+if __name__ == "__main__": 
+    args = get_args()
+
+    model, collision_model, visual_model, data = heron_approximation(args)
+    print(model)
+    viz = MeshcatVisualizer(model, collision_model, visual_model)
+    viz.initViewer(open=True)
+    viz.loadViewerModel()
+    q = np.zeros(model.nq)
+    theta = np.random.random() * 2*np.pi
+    q[2] = np.cos(theta)
+    q[3] = np.sin(theta)
+    v = np.random.random(model.nv)
+    #q = np.random.random(model.nq) * 0.5
+    for i in range(10000):
+        q = pin.integrate(model, q, v * 0.001)
+        viz.display(q)
+
+#    robot = RobotManager(args)
+#    Mgoal = robot.defineGoalPointCLI()
+#    compliantMoveL(args, robot, Mgoal)
+#    robot.closeGripper()
+#    robot.openGripper()
+#    if not args.pinocchio_only:
+#        robot.stopRobot()
+#
+#    if args.visualize_manipulator:
+#        robot.killManipulatorVisualizer()
+#    
+#    if args.save_log:
+#        robot.log_manager.saveLog()
+#    #loop_manager.stopHandler(None, None)
+#