diff --git a/README.md b/README.md
index 918c13473fe30ea0361e02a933a87006b76a41d5..ac2a5832eb48ecb23f93852063ec07cd805c82fe 100644
--- a/README.md
+++ b/README.md
@@ -114,8 +114,6 @@ The library is also shipped as a Docker image. Dockerfile available in docker fo
# Usage on real robots
---------------------
-## connecting to the real robot
--------------------------------
### UR robots
The ur_rtde interface communicates with the robot over a network. It connects to the 50002 port on the robot.
If you computer and the robot are on the same network, and you set the robot ip with the --robot-ip argument,
diff --git a/development_plan.toml b/development_plan.toml
index d1c908450d435e98a275dd1669033a377e2c636d..350e4389dbeeb65d918b25279595246c1992d7cb 100644
--- a/development_plan.toml
+++ b/development_plan.toml
@@ -233,14 +233,14 @@ deadline = 2025-01-31
dependencies = []
purpose = """actually pre-verifying experiments with simulation"""
hours_required = 2
-is_done = false
+is_done = true
[[project_plan.action_items.action_items]]
name = "refactor croco mpc p2p"
description = """
you have 4 or 5 literally identical controlloops and it looks (is) stupid. the circle to square is that they need to take in different arguments due to the templates. find how to intelligently package this.
"""
-priority = 1
+priority = 2
deadline = 2025-01-31
dependencies = []
purpose = """actually pre-verifying experiments with simulation"""
@@ -471,7 +471,7 @@ purpose = """makes you sleep safe at night, saves a lot of time after writing of
you want to debug everything immediately while the changes made are still fresh - it's
much more annoying if it happens afterward"""
hours_required = 10
-is_done = false
+is_done = true
[[project_plan.action_items.action_items]]
name = "tidy up typing"
diff --git a/examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py b/examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f1fa1bb4734f9c625b469ea9e3a4d469daf8108
--- /dev/null
+++ b/examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py
@@ -0,0 +1,57 @@
+from smc import getRobotFromArgs
+from smc import getMinimalArgParser
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+from smc.control.optimal_control.croco_mpc_point_to_point import (
+ CrocoDualEEAndBaseP2PMPC,
+)
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from smc.util.define_random_goal import getRandomlyGeneratedGoal
+
+from pinocchio import SE3
+import numpy as np
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ parser = getClikArgs(parser) # literally just for goal error
+ args = parser.parse_args()
+ return args
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+ assert issubclass(robot.__class__, MobileBaseInterface)
+ assert issubclass(robot.__class__, DualArmInterface)
+ # TODO: put this back for nicer demos
+ # Mgoal = defineGoalPointCLI()
+ T_w_absgoal = getRandomlyGeneratedGoal(args)
+ T_absgoal_l = SE3.Identity()
+ T_absgoal_l.translation[1] = 0.1
+ T_absgoal_r = SE3.Identity()
+ T_absgoal_r.translation[1] = -0.1
+ p_basegoal = T_w_absgoal.translation.copy()
+ p_basegoal += np.random.random(3) / 4
+ p_basegoal[2] = 0.0
+
+ if args.visualizer:
+ # TODO: document defined viz messages somewhere
+ robot.updateViz({"fixed_path": p_basegoal.reshape((1, 3))})
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_absgoal})
+
+ CrocoDualEEAndBaseP2PMPC(
+ args, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r, p_basegoal
+ )
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
diff --git a/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py b/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
index e00a3cf50d3328fc8a03362b57f7fe3469f7c08d..8fd3c4c0e98dc6e383d9a30d910ce39c8430c98e 100644
--- a/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
+++ b/examples/optimal_control/point_to_point/croco_base_and_ee_reference_p2p.py
@@ -23,8 +23,6 @@ if __name__ == "__main__":
robot = getRobotFromArgs(args)
assert issubclass(robot.__class__, MobileBaseInterface)
assert issubclass(robot.__class__, SingleArmInterface)
- robot._step()
- robot._step()
# TODO: put this back for nicer demos
# Mgoal = defineGoalPointCLI()
T_w_goal = SE3.Random()
diff --git a/examples/optimal_control/point_to_point/test_by_running.sh b/examples/optimal_control/point_to_point/test_by_running.sh
index 82ebcb720f11aa95119e75d3b609cfca5f6f0265..d1d004230990f71fb5f70b24311cfc45ffac4033 100755
--- a/examples/optimal_control/point_to_point/test_by_running.sh
+++ b/examples/optimal_control/point_to_point/test_by_running.sh
@@ -49,10 +49,17 @@ python $runnable
# whole body single arm - base + ee reference
# ----------------------------------
# ocp TODO: missing
-# mpc TODO: missing
+#
+# mpc
+runnable="croco_base_and_ee_reference_p2p.py --max-solver-iter 10 --n-knots 30 --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
+echo $runnable
+python $runnable
+#
#
# whole body dual arm base + ee reference
# ----------------------------------
# ocp TODO: missing
-# mpc TODO: missing
-#
+# mpc
+runnable="croco_base_and_dual_ee_reference_p2p.py --max-solver-iter 10 --n-knots 30 --robot=myumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
+echo $runnable
+python $runnable
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
index c4937fdf1c0ccf1e3b49d419d40dde088c2ceedf..96e334e7f57214ee60136311428c0402011be31f 100644
--- a/python/smc/control/controller_templates/point_to_point.py
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -16,6 +16,7 @@ from smc.robots.interfaces.whole_body_interface import (
global control_loop_return
control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
+
def BaseP2PCtrlLoopTemplate(
SOLVER: Any,
p_basegoal: np.ndarray,
@@ -55,9 +56,7 @@ def BaseP2PCtrlLoopTemplate(
p_base = np.array(list(robot.q[:2]) + [0.0])
base_err_vector_norm = np.linalg.norm(p_basegoal - p_base)
- if (
- base_err_vector_norm < robot.args.goal_error
- ):
+ if base_err_vector_norm < robot.args.goal_error:
breakFlag = True
log_item["qs"] = robot.q
diff --git a/python/smc/control/optimal_control/croco_mpc_point_to_point.py b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
index 061ea5076db5210b04f1997606fe3db56a047e24..379a8c4b1e4370d56bdb2f22dc528da211a3b92b 100644
--- a/python/smc/control/optimal_control/croco_mpc_point_to_point.py
+++ b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
@@ -28,6 +28,7 @@ from functools import partial
from collections import deque
from argparse import Namespace
+
def CrocoBaseP2PMPCControlLoop(
ocp: CrocoOCP,
p_basegoal: np.ndarray,
@@ -62,7 +63,12 @@ def CrocoBaseP2PMPC(
ocp.solveInitialOCP(x0)
controlLoop = partial(
- BaseP2PCtrlLoopTemplate, ocp, p_basegoal, CrocoBaseP2PMPCControlLoop, args, robot
+ BaseP2PCtrlLoopTemplate,
+ ocp,
+ p_basegoal,
+ CrocoBaseP2PMPCControlLoop,
+ args,
+ robot,
)
log_item = {
"qs": np.zeros(robot.nq),
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index 990ceb0ae8cea93905537ccff5c540caf7bfd805..d322e996bdac355da570fcae6c0bbcf3e3fe11e7 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -79,6 +79,8 @@ class SimulatedHeronRobotManager(
)
# pin.RandomConfiguration does not work well for planar joint,
# or at least i remember something along those lines being the case
+ self._q[0] = np.random.random()
+ self._q[1] = np.random.random()
theta = np.random.random() * 2 * np.pi - np.pi
self._q[2] = np.cos(theta)
self._q[3] = np.sin(theta)
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 76c54c1f4c7c20df8cf579a564fb8ce7429e2925..28af048a563e96f679d7988996ef86faf8b46869 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -55,6 +55,8 @@ class SimulatedMobileYuMiRobotManager(
)
# pin.RandomConfiguration does not work well for planar joint,
# or at least i remember something along those lines being the case
+ self._q[0] = np.random.random()
+ self._q[1] = np.random.random()
theta = np.random.random() * 2 * np.pi - np.pi
self._q[2] = np.cos(theta)
self._q[3] = np.sin(theta)
diff --git a/python/smc/robots/interfaces/whole_body_interface.py b/python/smc/robots/interfaces/whole_body_interface.py
index 417ab62bf74c3db83284523ebfe393623ff64094..6e81f41f01890349c1640bf1b2bf6b2ed0eca13d 100644
--- a/python/smc/robots/interfaces/whole_body_interface.py
+++ b/python/smc/robots/interfaces/whole_body_interface.py
@@ -308,6 +308,8 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
"""
assert type(v_cmd) == np.ndarray
v_cmd_to_real = np.zeros(self.model.nv)
+ if self._mode == self.control_mode.whole_body:
+ v_cmd_to_real = v_cmd
if self._mode == self.control_mode.base_only:
v_cmd_to_real[:3] = v_cmd
if self._mode == self.control_mode.upper_body: