From bce29a1d99a42b29bcd6acc6897927b6afb1579e Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 7 Mar 2025 11:44:16 +0100
Subject: [PATCH] tests for p2p mpcs are there
---
README.md | 2 -
development_plan.toml | 6 +-
.../croco_base_and_dual_ee_reference_p2p.py | 57 +++++++++++++++++++
.../croco_base_and_ee_reference_p2p.py | 2 -
.../point_to_point/test_by_running.sh | 13 ++++-
.../controller_templates/point_to_point.py | 5 +-
.../croco_mpc_point_to_point.py | 8 ++-
python/smc/robots/implementations/heron.py | 2 +
.../smc/robots/implementations/mobile_yumi.py | 2 +
.../robots/interfaces/whole_body_interface.py | 2 +
10 files changed, 85 insertions(+), 14 deletions(-)
create mode 100644 examples/optimal_control/point_to_point/croco_base_and_dual_ee_reference_p2p.py
diff --git a/README.md b/README.md
index 918c134..ac2a583 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 d1c9084..350e438 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 0000000..1f1fa1b
--- /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 e00a3cf..8fd3c4c 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 82ebcb7..d1d0042 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 c4937fd..96e334e 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 061ea50..379a8c4 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 990ceb0..d322e99 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 76c54c1..28af048 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 417ab62..6e81f41 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:
--
GitLab