diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index 90cd8498bcdc42ac3ab90468d6e3de634bb8ad86..a8ac8f3dc4666171dc141fcdf28735fbac24baf1 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -11,6 +11,7 @@ from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.clik.clik import getClikArgs
import pinocchio as pin
import crocoddyl
+import mim_solvers
def get_args():
@@ -62,7 +63,8 @@ if __name__ == "__main__":
print("final position:")
print(robot.getT_w_e())
- robot.log_manager.plotAllControlLoops()
+ if args.save_log:
+ robot.log_manager.plotAllControlLoops()
if not args.pinocchio_only:
robot.stopRobot()
diff --git a/python/examples/data/latest_run b/python/examples/data/latest_run
index ff992b84184343c3d9e4f6270f795b27a195a77f..4847c67d6795dc1aaf0da33991c1bbb123c3e83d 100644
Binary files a/python/examples/data/latest_run and b/python/examples/data/latest_run differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 88c6cb821ec8dc97c3b71076271c9edadd6dadf8..0a834918a14e09f0bd0fb57d66922f2d7dd23576 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 3decba093e80e1377fe12691c3dd30f7a974897a..f5e0926553fb34fa0eba892b80f22fce73f3740b 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -138,7 +138,7 @@ def getClikController(args):
# modularity yo
# past data to comply with the API
# TODO make this a kwarg or whatever to be more lax with this
-def controlLoopClik(robot, clik_controller, i, past_data):
+def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
"""
controlLoopClik
---------------
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index f96d0e303ebf81b3593e469ad4ffc057cbcb84b9..afacc9c87f178f5b16d896efd355eaf68ca5207e 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -466,6 +466,7 @@ class RobotManager:
# and since this is unintuitive, we add the other variable too
# so that the control designer doesn't need to think about such bs
self.JOINT_ID = 6
+ self.ee_frame_id = self.model.getFrameId("tool0")
self.update_rate = 500 #Hz
self.dt = 1 / self.update_rate
# you better not give me crazy stuff
@@ -571,7 +572,10 @@ class RobotManager:
# which is important in situations like this)
pin.forwardKinematics(self.model, self.data, q_given,
np.zeros(self.model.nv), np.zeros(self.model.nv))
- return self.data.oMi[self.JOINT_ID].copy()
+ # NOTE: this also returns the frame, so less copying possible
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ #return self.data.oMi[self.JOINT_ID].copy()
+ return self.data.oMf[self.ee_frame_id].copy()
# this is in EE frame by default (handled in step which
@@ -648,7 +652,9 @@ class RobotManager:
# NOTE: it's too slow -> has to have pinocchio with multithreading
#pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
- self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ #self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
+ self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
# wrench in EE should obviously be the default
self._getWrenchInEE(step_called=True)
# this isn't real because we're on a velocity-controlled robot,
@@ -746,8 +752,10 @@ class RobotManager:
q = np.array(q)
self.q = q
pin.forwardKinematics(self.model, self.data, q)
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
# TODO probably remove deepcopy
- self.T_w_e = self.data.oMi[self.JOINT_ID]
+ #self.T_w_e = self.data.oMi[self.JOINT_ID]
+ self.T_w_e = self.data.oMi[self.ee_frame_id]
def _getQd(self):
"""
@@ -900,7 +908,9 @@ class RobotManager:
q = self.getQ()
# define goal
pin.forwardKinematics(self.model, self.data, np.array(q))
- T_w_e = self.data.oMi[self.JOINT_ID]
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ #T_w_e = self.data.oMi[self.JOINT_ID]
+ T_w_e = self.data.oMf[self.ee_frame_id]
print("You can only specify the translation right now.")
if not self.pinocchio_only:
print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 833a629e83be093317c49121020419c044232609..d766a1b24bd2062a496a1d0a71a2cd330b09385b 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -2,6 +2,7 @@ from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager,
from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP
import pinocchio as pin
import crocoddyl
+import mim_solvers
import numpy as np
from functools import partial
@@ -22,7 +23,6 @@ from functools import partial
# and probably better.
# i'm pretty sure that's how croco devs & mim_people do mpc
-# TODO: FINISH
def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data):
breakFlag = False
log_item = {}