diff --git a/development_plan.toml b/development_plan.toml
index f5abe0ab006585a4d3aa45818c4de48bc32cb2df..54761b2ccf8b39cbcc6b6a1bbdeb75aa7aed252c 100644
--- a/development_plan.toml
+++ b/development_plan.toml
@@ -222,6 +222,19 @@ purpose = """reducing the amount of code, thereby improving extensibility and ma
hours_required = 5
is_done = true
+[[project_plan.action_items.action_items]]
+name = "fix (add) --start-from-current-pose feature"
+description = """
+crucial feature to connect simulation and running on the real robot.
+a bit iffy now due to the separation of abstract, real and simulated robot manager. a hack function on the side can get the job done.
+"""
+priority = 1
+deadline = 2025-01-31
+dependencies = []
+purpose = """actually pre-verifying experiments with simulation"""
+hours_required = 2
+is_done = false
+
[[project_plan.action_items]]
name = "improving extensibility"
diff --git a/examples/cart_pulling/todos.toml b/examples/cart_pulling/todos.toml
index e07e154764643a42d3e9c45f750d40d8273936e5..3d59d00cef92fd5e51f80eb843af954f12676c90 100644
--- a/examples/cart_pulling/todos.toml
+++ b/examples/cart_pulling/todos.toml
@@ -181,11 +181,8 @@ we get a bitmap of the map from some ros2 topic. this needs to be starified for
the todos go as follows:
1) figure out how to get a matrix occupancy grid or whatever format from ros2.
-2) know which format the path planner wants. it's something in shapely, which is then mapped into star shapes.
- the outcome here is knowing which shapes are already supported and running with that.
- just rectangles and ellipses should be good enough
-3) create a fixed transformation for the static map of the lab into star shapes. ideally
- save that and keep it cached.
+2) know which format the path planner wants. it's something in shapely, which is then mapped into star shapes. the outcome here is knowing which shapes are already supported and running with that. just rectangles and ellipses should be good enough
+3) create a fixed transformation for the static map of the lab into star shapes. ideally save that and keep it cached.
4) (optional, but highly preferable) take what isn't static in the occupancy map, convex hull it, hit a rectangle/ellipse on that
"""
priority = 1
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index 5a298331264f5652bc783c2b08040321095664ec..a8da284d661e89b9252b98276ceb14b4c919c5a9 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -6,6 +6,7 @@ from argparse import Namespace
import pinocchio as pin
import numpy as np
from functools import partial
+from enum import Enum
class AbstractRobotManager(abc.ABC):
@@ -20,11 +21,31 @@ class AbstractRobotManager(abc.ABC):
- provide functions every robot has to have like getQ
"""
+ # NOTE: these are all the possible modes for all possible robots.
+ # all are declared here to enforce uniformity in child classes.
+ # each robot has to declare which of these it supports.
+ control_mode = Enum(
+ "mode",
+ [
+ ("whole_body", 1),
+ ("base_only", 2),
+ ("arm_only", 3),
+ ("both_arms", 3),
+ ("both_arms_only", 3),
+ ("left_arm_only", 4),
+ ("right_arm_only", 5),
+ ],
+ )
+
def __init__(self, args: Namespace):
self._model: pin.Model
self._data: pin.Data
self._visual_model: pin.pinocchio_pywrap_default.GeometryModel
self._collision_model: pin.pinocchio_pywrap_default.GeometryModel
+ self._mode: AbstractRobotManager.control_mode = (
+ AbstractRobotManager.control_mode.whole_body
+ )
+ self._available_modes: list[AbstractRobotManager.control_mode]
self.args: Namespace = args
if args.debug_prints:
print("AbstractRobotManager init")
@@ -69,6 +90,10 @@ class AbstractRobotManager(abc.ABC):
self.gripper = None
# initialize things that depend on q here
# self._step()
+ # check for where args.mode is legal is handled by argparse
+ for mode in self.control_mode:
+ if mode.name == args.mode:
+ self.mode = mode
####################################################################
# processes and utilities robotmanager owns #
@@ -101,6 +126,15 @@ class AbstractRobotManager(abc.ABC):
"sorry, almost done - look at utils to get 90% of the solution!"
)
+ @property
+ def mode(self) -> control_mode:
+ return self._mode
+
+ @mode.setter
+ def mode(self, mode: control_mode) -> None:
+ assert mode in self._available_modes
+ self._mode = mode
+
@property
def model(self) -> pin.Model:
return self._model
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index 02916b7eb4a567c34b6a61e8e9a1f2d273e5d691..6087d3fa0026de5531d78c7052a86bcd35767951 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -29,11 +29,10 @@ class AbstractHeronRobotManager(
):
def __init__(self, args):
if args.debug_prints:
- print("RobotManagerHeron init")
+ print("AbstractHeronRobotManager init")
self._model, self._collision_model, self._visual_model, self._data = (
heron_approximation()
)
- self._mode = SingleArmWholeBodyInterface.control_mode.whole_body
self._ee_frame_id = self.model.getFrameId("tool0")
self._base_frame_id = self.model.getFrameId("mobile_base")
# TODO: CHANGE THIS TO REAL VALUES
@@ -43,7 +42,7 @@ class AbstractHeronRobotManager(
class SimulatedHeronRobotManager(
- AbstractSimulatedRobotManager, AbstractHeronRobotManager
+ AbstractHeronRobotManager, AbstractSimulatedRobotManager
):
def __init__(self, args: Namespace):
if args.debug_prints:
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index 5bd709a4d75462122383134501450ef7de937f3b..b04f82afc183e9f90edff7c9a62a726c3f80b887 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -1,9 +1,9 @@
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.abstract_robotmanager import AbstractRobotManager
import numpy as np
import pinocchio as pin
from argparse import Namespace
-from enum import Enum
class DualArmInterface(SingleArmInterface):
@@ -15,10 +15,6 @@ class DualArmInterface(SingleArmInterface):
- r stands for right
"""
- control_mode = Enum(
- "mode", [("both_arms", 3), ("left_arm_only", 4), ("right_arm_only", 5)]
- )
-
# NOTE: you need to fill in the specific names from the urdf here for your robot
# (better than putting in a magic number)
# self._l_ee_frame_id = self.model.getFrameId("left_ee_name")
@@ -34,15 +30,11 @@ class DualArmInterface(SingleArmInterface):
if args.debug_prints:
print("DualArmInterface init")
super().__init__(args)
-
- @property
- def mode(self) -> control_mode:
- return self._mode
-
- @mode.setter
- def setMode(self, mode: control_mode) -> None:
- assert type(mode) is self.control_mode
- self._mode = mode
+ self._available_modes: list[AbstractRobotManager.control_mode] = [
+ AbstractRobotManager.control_mode.both_arms,
+ AbstractRobotManager.control_mode.left_arm_only,
+ AbstractRobotManager.control_mode.right_arm_only,
+ ]
@property
def q(self) -> np.ndarray:
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index 4f037718f96839a3d313b105c9188315aee2efa6..f82335b80573c64a2e6995446a0a43a12f237ebf 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -10,6 +10,9 @@ class SingleArmInterface(AbstractRobotManager):
self._ee_frame_id: int
self._T_w_e: pin.SE3
super().__init__(args)
+ self._available_modes: list[AbstractRobotManager.control_mode] = [
+ AbstractRobotManager.control_mode.whole_body,
+ ]
@property
def ee_frame_id(self):
diff --git a/python/smc/robots/interfaces/whole_body_interface.py b/python/smc/robots/interfaces/whole_body_interface.py
index d96826b4381c227827df912572ed8af5bbe7ebd2..1cc39229c26b5bfe1dee4778ed87281665f2de56 100644
--- a/python/smc/robots/interfaces/whole_body_interface.py
+++ b/python/smc/robots/interfaces/whole_body_interface.py
@@ -1,3 +1,4 @@
+from smc.robots.abstract_robotmanager import AbstractRobotManager
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.robots.interfaces.dual_arm_interface import DualArmInterface
@@ -23,12 +24,15 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
what you get depends on the mode you set - they're enumerate as above
"""
- control_mode = Enum("mode", [("whole_body", 1), ("base_only", 2), ("arm_only", 3)])
-
def __init__(self, args: Namespace):
if args.debug_prints:
print("SingleArmWholeBodyInterface init")
- self._mode: SingleArmWholeBodyInterface.control_mode
+ self._mode: AbstractRobotManager.control_mode
+ self._available_modes: list[AbstractRobotManager.control_mode] = [
+ AbstractRobotManager.control_mode.whole_body,
+ AbstractRobotManager.control_mode.base_only,
+ AbstractRobotManager.control_mode.arm_only,
+ ]
super().__init__(args)
# TODO: override model property to produce the reduced version
@@ -37,14 +41,10 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
# NOTE: that this is currently only important for ocp construction,
# even though it's obviously the correct move either way
- @property
- def mode(self) -> control_mode:
- return self._mode
-
- @mode.setter
- def setMode(self, mode: control_mode) -> None:
- assert type(mode) is self.control_mode
- self._mode = mode
+ # @AbstractRobotManager.mode.setter
+ # def mode(self, mode: AbstractRobotManager.control_mode) -> None:
+ # assert type(mode) in self._available_modes
+ # self._mode = mode
# TODO: put back in when python3.12 will be safe to use
# @override
@@ -138,22 +138,17 @@ class SingleArmWholeBodyInterface(SingleArmInterface, MobileBaseInterface):
class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
- control_mode = Enum(
- "mode",
- [
- ("whole_body", 1),
- ("base_only", 2),
- ("both_arms_only", 3),
- ("left_arm_only", 4),
- ("right_arm_only", 5),
- ],
- )
-
def __init__(self, args: Namespace):
if args.debug_prints:
print("DualArmWholeBodyInterface init")
- self._mode: DualArmWholeBodyInterface.control_mode
super().__init__(args)
+ self._available_modes: list[AbstractRobotManager.control_mode] = [
+ AbstractRobotManager.control_mode.whole_body,
+ AbstractRobotManager.control_mode.base_only,
+ AbstractRobotManager.control_mode.both_arms_only,
+ AbstractRobotManager.control_mode.left_arm_only,
+ AbstractRobotManager.control_mode.right_arm_only,
+ ]
# TODO: override model property to produce the reduced version
# depending on the control mode.
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index faf7c5c4942d4af97cb1c2a1b4b4acc84e21e13d..95710f64249ecdf397f3af41361015f00ee6f4ab 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -30,6 +30,21 @@ def getMinimalArgParser():
default="ur5e",
choices=["ur5e", "heron", "yumi", "myumi", "mir"],
)
+ parser.add_argument(
+ "--mode",
+ type=str,
+ help="control mode the robot should run in, i.e. select if you're controlling only a subset of the available joints. most of these only make sense for mobile robots with arms of course (you get an error upon an erroneous selection)",
+ default="whole_body",
+ choices=[
+ "whole_body",
+ "base_only",
+ "arm_only",
+ "both_arms",
+ "both_arms_only",
+ "left_arm_only",
+ "right_arm_only",
+ ],
+ )
parser.add_argument(
"--real",
action=argparse.BooleanOptionalAction,