diff --git a/development_plan.toml b/development_plan.toml
index dde571a98043dfc0ba0c935d874c5252d1729250..f5abe0ab006585a4d3aa45818c4de48bc32cb2df 100644
--- a/development_plan.toml
+++ b/development_plan.toml
@@ -220,25 +220,91 @@ deadline = 2025-01-31
dependencies = ["explicit relationship between controlLoop and ControlLoopManager"]
purpose = """reducing the amount of code, thereby improving extensibility and maintainability"""
hours_required = 5
+is_done = true
+
+
+[[project_plan.action_items]]
+name = "improving extensibility"
+description = """
+there are half-baked or imperfect features in the library. while they do their job OK for now, in the future i will forget about the shitty quirks, will want to add a new feature and will then waste a lot of time first trying to remember how a thing works, then re-writing it, then fighting the causes of the quirks, and only then making the feature i want. so the process will take 3x take then it should. it's better to do the re-design now rather than later
+"""
+priority = 2
+deadline = 2025-01-31
+dependencies = []
+purpose = """extensibility is good"""
+hours_required = 5
is_done = false
+[[project_plan.action_items.action_items]]
+name = "refactor processmanager"
+description = """
+one process manager for every communication protocol is not great and is already a bit out of hand. processmanager should be an abstract class, which is then specialized into "shared_memory_comm_process_manager" or whatever else.
+"""
+priority = 2
+deadline = 2025-01-31
+dependencies = ["explicit relationship between controlLoop and ControlLoopManager"]
+purpose = """reducing the amount of code, thereby improving extensibility and maintainability"""
+hours_required = 6
+is_done = false
[[project_plan.action_items.action_items]]
-name = "actual process management"
+name = "debug storing protobuf in shared memory"
description = """
-plotter process being owned by controlloopmanager, and visualizer process being owned by robotmanager works fine.
+for some reason i can't read the serialized protobuf message after storing it in memory, so i deserialize the protobuf, and then pickle the message to store it in shm. this is obviously idiotic, but there is something evil in the python's hared memory and protobuf.
-however, we now have more processes to manage and it's not obvious who should take ownership of them: camera (computer vision) process, networking processes, and future ones which will be added (doing more stuff in more processes seems to be a trend). forcing the user to manually terminate them or to keep adding try-except KeyboardInterrupt blocks dramatically decreases the value of the library because all this is annoying as fuck and makes it seems broken.
+in any event it has to be fixed
+"""
+priority = 2
+deadline = 2025-01-31
+dependencies = []
+purpose = """removing clearly bad things from codebase"""
+hours_required = 5
+is_done = false
-so the bottom line is we need an actual process management class - not for individual processes, but for all of them. this class should, upon termination, do what stophandler in controlloopmanager is doing now. it should be extensible in the sense that you can register however many processes, and it takes ownership of all of them.
+[[project_plan.action_items.action_items]]
+name = "slight client-server refactoring"
+description = """
+call them publisher and subscriber, or server-publisher, server-subsriber, client-publisher and client-subscriber.
-one possibly problematic thing is that control might have to be moved from the main thread, which immediately makes everything annoying - the transition between different controllers has to be on the same thread. writing examples must not be polluted with writing one massive function that's then passed ran as a side process. idk, think of what to do here as a part of the todo.
+also document them better and put in instruction on compiling protobuf messages.
"""
priority = 2
deadline = 2025-01-31
+dependencies = ["debug storing protobuf in shared memory"]
+purpose = """good-enough networking from workable half-baked networking"""
+hours_required = 4
+is_done = false
+
+[[project_plan.action_items.action_items]]
+name = "python stdlib logging instead of printing"
+description = """
+when just printing the --debug-prints, they are all of the same color,
+and all of same importance. obviously they are not. proper loging
+enables much more possible prints because you can set printing level
+(without 4 ifs per print statement), has colors, has everything you want here.
+
+more importantly, you need to allow different levels of verbosity, or maybe even different variants.
+printing everything or nothing as it is now is already fucked (ex. optimal control printouts eat up everything else)
+"""
+priority = 3
+deadline = 2026-09-01
+dependencies = []
+purpose = """making debug prints substantially more useful and friendly"""
+# -1 means it should be a sum over sub-items
+hours_required = 6
+is_done = false
+
+
+[[project_plan.action_items]]
+name = "extending library scope"
+description = """
+extending the library to do more than what its current structure supports. the proposed features here require restructuring of the library, probably breaking the current API.
+"""
+priority = 2
+deadline = 2025-09-01
dependencies = ["robotmanager abstract class"]
-purpose = """actual process management beyond ducktaping plotter and visualizer to loopmanager and robotmanager"""
-hours_required = 5
+purpose = """depends on the feature"""
+hours_required = 0
is_done = false
@@ -249,15 +315,63 @@ some robots are torque controlled, some are velocity controlled.
to make this be of practical use, torque-level controllers need to be written,
but since this will happen eventually, a template/interface for
can be made now. also the API for the torque-level controllers can be written out.
-it makes sense to do this within the refactoring task.
+
+the way to do this would be to add all the variables and function to all robots and then not use them if the robot doesn't support torque control. the reason why is because you can still simulate the dynamics, or even solve the control dynamically and then extracting (integrating, whatever) velocities to command them, like ocp does right now.
+
+NOTE: this is most likely to come into play for own papers requiring proper handling of dynamics.
"""
priority = 2
deadline = 2025-09-01
dependencies = ["robotmanager abstract class"]
purpose = """paving the way for torque-level control support"""
-hours_required = 3
+hours_required = 30
is_done = false
+[[project_plan.action_items.action_items]]
+name = "actual process management"
+description = """
+plotter process being owned by controlloopmanager, and visualizer process being owned by robotmanager works fine.
+
+however, we now have more processes to manage and it's not obvious who should take ownership of them: camera (computer vision) process, networking processes, and future ones which will be added (doing more stuff in more processes seems to be a trend). forcing the user to manually terminate them or to keep adding try-except KeyboardInterrupt blocks dramatically decreases the value of the library because all this is annoying as fuck and makes it seems broken.
+
+so the bottom line is we need an actual process management class - not for individual processes, but for all of them. this class should, upon termination, do what stophandler in controlloopmanager is doing now. it should be extensible in the sense that you can register however many processes, and it takes ownership of all of them.
+
+one possibly problematic thing is that control might have to be moved from the main thread, which immediately makes everything annoying - the transition between different controllers has to be on the same thread. writing examples must not be polluted with writing one massive function that's then passed ran as a side process. idk, think of what to do here as a part of the todo.
+"""
+priority = 2
+deadline = 2025-01-31
+dependencies = ["robotmanager abstract class"]
+purpose = """actual process management beyond ducktaping plotter and visualizer to loopmanager and robotmanager"""
+hours_required = 5
+is_done = false
+
+[[project_plan.action_items.action_items]]
+name = "adding interface to a physics simulator"
+description = """
+for some tasks, having a proper physics simulator is essential.
+it seems the easiest to interface with pybullet, and
+you can refer to how mim people did it.
+"""
+priority = 3
+deadline = 2025-09-01
+dependencies = []
+engineering_purpose = """text"""
+personal_purpose = """text"""
+# -1 means it should be a sum over sub-items
+hours_required = 16
+is_done = false
+
+[[project_plan.action_items]]
+name = "improving portability"
+description = """
+you need to do this if you want other people to use this library.
+"""
+priority = 2
+deadline = 2025-09-01
+dependencies = []
+purpose = """getting this project off the ground, essential for its long term life"""
+hours_required = 0
+is_done = false
[[project_plan.action_items.action_items]]
name = "modularity and dependencies"
@@ -278,8 +392,37 @@ easier maintenance. most important for not relying on ros"""
hours_required = 2
is_done = false
+[[project_plan.action_items.action_items]]
+name = "resolving python packaging"
+description = """
+the packaging of the library, as it is now, is barely functional.
+it does not have dependencies clearly written down.
+every import is miles long because you need to go through every possible directory
+to get anywhere and upon importing in a script it's overly messy for no go reason.
+"""
+priority = 3
+deadline = 2026-09-01
+dependencies = []
+purpose = """fixing long importing, autocompletion, and necessary for professional-grade shipping"""
+# -1 means it should be a sum over sub-items
+hours_required = 6
+is_done = false
[[project_plan.action_items]]
+name = "improving maintainability"
+description = """
+maintainability is not important just for using the library in the future, but also to make the process of adding new feature smoother.
+"""
+priority = 1
+deadline = 2025-09-01
+dependencies = []
+purpose = """maintainability is not important just for using the library in the future, but also to make the process of adding new feature smoother.
+"""
+hours_required = 0
+is_done = false
+
+
+[[project_plan.action_items.action_items]]
name = "writing tests"
description = """
write some tests too see that:
@@ -296,7 +439,7 @@ with some different parameters - finish that. this checks whether anything runs,
and whether things converge. probably a good idea to run some of these multiple times
in the case they are not supposed to converge every time (if you have crazy init conditions etc).
"""
-priority = 2
+priority = 1
deadline = ["before going to vasteros", 2025-02-10]
dependencies = ["smc refactor"]
purpose = """makes you sleep safe at night, saves a lot of time after writing of a feature -
@@ -305,24 +448,21 @@ much more annoying if it happens afterward"""
hours_required = 10
is_done = false
-
-[[project_plan.action_items]]
-name = "adding interface to a physics simulator"
+[[project_plan.action_items.action_items]]
+name = "tidy up typing"
description = """
-for some tasks, having a proper physics simulator is essential.
-it seems the easiest to interface with pybullet, and
-you can refer to how mim people did it.
+sit down and get typing coverage to 100% or whatever is the limit of reason.
"""
priority = 3
deadline = 2025-09-01
-dependencies = []
-engineering_purpose = """text"""
-personal_purpose = """text"""
+dependencies = ["create/fix type-checking workflow"]
+purpose = """easier code writting and debugging, making library professional-grade"""
# -1 means it should be a sum over sub-items
-hours_required = 16
+hours_required = 8
is_done = false
+
[[project_plan.action_items]]
name = "rewrite into cpp"
description = """
@@ -341,34 +481,7 @@ street cred"""
hours_required = 80
is_done = false
-[[project_plan.action_items]]
-name = "tidy up typing"
-description = """
-sit down and get typing coverage to 100% or whatever is the limit of reason.
-"""
-priority = 3
-deadline = 2025-09-01
-dependencies = ["create/fix type-checking workflow"]
-purpose = """easier code writting and debugging, making library professional-grade"""
-# -1 means it should be a sum over sub-items
-hours_required = 8
-is_done = false
-[[project_plan.action_items]]
-name = "python stdlib logging instead of printing"
-description = """
-when just printing the --debug-prints, they are all of the same color,
-and all of same importance. obviously they are not. proper loging
-enables much more possible prints because you can set printing level
-(without 4 ifs per print statement), has colors, has everything you want here.
-"""
-priority = 3
-deadline = 2026-09-01
-dependencies = []
-purpose = """making debug prints substantially more useful and friendly"""
-# -1 means it should be a sum over sub-items
-hours_required = 4
-is_done = false
[[project_plan.action_items]]
@@ -388,25 +501,10 @@ hours_required = 20
is_done = false
-[[project_plan.action_items]]
-name = "resolving python packaging"
-description = """
-the packaging of the library, as it is now, is barely functional.
-it does not have dependencies clearly written down.
-every import is miles long because you need to go through every possible directory
-to get anywhere and upon importing in a script it's overly messy for no go reason.
-"""
-priority = 3
-deadline = 2026-09-01
-dependencies = []
-purpose = """fixing long importing, autocompletion, and necessary for professional-grade shipping"""
-# -1 means it should be a sum over sub-items
-hours_required = 6
-is_done = false
[[project_plan.action_items]]
-name = "fixing and improving documentation"
+name = "writing documentation"
description = """
all functions need to be documented. some need to be better documented.
there has to be a uniform documentation standard,
@@ -424,7 +522,7 @@ but a lot of things do need to be explain in text and video format.
re-doing videos with scripts is a project by itself,
this todo concerns the documenting of the codebase (but the two are obviously related).
"""
-priority = 2
+priority = 1
deadline = 2026-09-01
dependencies = []
purpose = """essential for getting other people to use the library, but not as essential
@@ -433,6 +531,59 @@ as killer features if they aren't written yet"""
hours_required = 0
is_done = false
+[[project_plan.action_items.action_items]]
+name = "documenting functions and classes"
+description = """
+write the short documentation in the string below function/class declaration
+"""
+priority = 1
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """documentation for active code writing"""
+hours_required = 6
+
+[[project_plan.action_items.action_items]]
+name = "documenting examples"
+description = """
+we started writing technical reports. this is perfect documentation for an example. describe the tradeoffs, controller selection and other engineering decisions.
+
+NOTE: when writing the technical report, you must include how, where and which to particular script to run with what arguments to reproduce the result.
+NOTE: you HAVE TO describe the all the arguments/parameters, what they do and what reasonable values are. preferably support this with asserts in the code - one shouldn't be able to accidentaly blow up everything by selecting a bad parameter. it would also be good if different defaults sat in appropriate yaml files
+"""
+priority = 1
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """documentation for what you can do with the library, but also for my work more generally"""
+hours_required = 0
+
+[[project_plan.action_items.action_items.action_items]]
+name = "documenting drawing example"
+description = """
+write about it man
+"""
+priority = 1
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """fully finish the drawing demo until the end of time"""
+hours_required = 6
+
+
+[[project_plan.action_items.action_items.action_items]]
+name = "old student projects"
+description = """
+if possible try making it runnable and save it for posterity's sake. the documentatio is their reports of course.
+you most certainly don't want all of it, nor do you want to waste time on it, but at least one would be good.
+"""
+priority = 3
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """fully finish the drawing demo until the end of time"""
+hours_required = 6
+
[[project_plan.action_items]]
name = "more controllers"
@@ -486,6 +637,31 @@ dependencies = []
purpose = """all clik is instantly improved. professional-grade solution too"""
hours_required = 6
+[[project_plan.action_items.action_items]]
+name = "vision stacks"
+description = """
+all students do computer vision effectively from scratch every time. this is a huge waste of time. the skeleton, some basic docs (could be links) and some basic things like detecting aruco codes should be included out of the box. vision should run as a separate process with processmanager where is stores whatever processing output in shared memory (what else could it possibly be)
+"""
+priority = 1
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """all clik is instantly improved. professional-grade solution too"""
+hours_required = 10
+
+
+[[project_plan.action_items.action_items]]
+name = "visual servoing done by yuyao"
+description = """
+yuyao's project course project did pushing via visual servoing. re-package this as an example. include the report as the technical report of course.
+"""
+priority = 1
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """all clik is instantly improved. professional-grade solution too"""
+hours_required = 6
+
[[project_plan.action_items.action_items]]
name = "teaching a skill via dmp's"
@@ -541,7 +717,7 @@ hours_required = 30
-[[project_plan.action_items]]
+[[project_plan.action_items.action_items]]
name = "behavior trees"
description = """
for the robot to do more complicated tasks, a behavior tree needs to be formed
diff --git a/examples/cart_pulling/todos.toml b/examples/cart_pulling/todos.toml
index 6712381f5935b9103909e69731584cb68f9db981..f47abe574573a280f5b89cfdeeca1584ec810cc1 100644
--- a/examples/cart_pulling/todos.toml
+++ b/examples/cart_pulling/todos.toml
@@ -14,6 +14,19 @@ hours_required = 0
is_done = false
[[project_plan.action_items]]
+name = "dealing with the real robots and their interfaces"
+description = """
+obvious task. includes not only dealing with the interface but also the mapping between smc and ros2
+"""
+priority = 1
+deadline = ["before going to vasteros", 2025-02-10]
+is_done = false
+dependencies = []
+purpose = """being able moving heron and yumi with code"""
+hours_required = 0
+assignee = "Marko"
+
+[[project_plan.action_items.action_items]]
name = "moving robots via their ROS interfaces"
description = """
heron's mobile base uses something/vel_cmd. there has to be a verification that this topic actually works. mobile yumi uses abb_whole_body_driver. this works in simulation, but not on the real robot. there are multiple possible reasons for this. solving this task means:
@@ -30,7 +43,7 @@ hours_required = 0
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "verifying vel_cmd and rest of heron interface"
description = """
make sure that we can control heron as expected. this boils down to verifying we can simultaneously move the base, the arm, and the gripper. the arm and the gripper should pose no problems as they should be decoupled from the base. moving the base means using something/vel_cmd to move the base.
@@ -47,7 +60,7 @@ purpose = """verify heron interface to be able to rest assuredly"""
hours_required = 2
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "exploring & understanding whole-body-driver"
description = """
the abb_myumi_whole_body_driver does not work on the real robot. this needs to be debugged, which in turn requires knowing what's going on. in particular, looking at the ins and outs of the driver, focusing on what needs to be (or musn't be) running for it to do actually move the robot.
@@ -67,7 +80,7 @@ hours_required = 12
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "finding alternatives to whole body driver"
description = """
the whole body driver might be doomed, so having a different way to control the robot is a good idea. this means controlling the arms and the base individually. the base is controllable through the nav2 stack as well. the arms are controllable through RAPID, but that's unusable. hopefully one can talk to egm of each arm directly. the point of this action item is to investigate that.
@@ -83,7 +96,7 @@ hours_required = 7
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "verifying operational frequency"
description = """
once both heron and myumi can be controlled, the control frequency needs to be determined. because both things run on ros2, and due to many processes running in parallel, it is difficult to say how often the commands are issued, and also whether there are consistent delays between them. this quantities should be determined if possible.
@@ -98,7 +111,7 @@ profiles etc)"""
hours_required = 4
assignee = "Marko"
-[[project_plan.action_items]]
+[[project_plan.action_items.action_items]]
name = "fast localization"
description = """
amcl from ros2 nav2 stack seems very good, but gives updates at 2Hz, which causes the robot to teleport internaly. for this reason, odometry should be used in-between updates, or even better, a kalman filter balancing odometry and amcl updates should be used. some amount of teleporting will certainly occur, but this can't be in meters for hopefully obvious reasons.
@@ -113,7 +126,7 @@ hours_required = 0
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "understanding and testing odometry"
description = """
ros2 nav2 odometry supposedly already incorporates information from position encoders on the wheel, imu data, lidar readings etc., but it of course does not use a map to localize. understanding odometry implies being able to answer the following:
@@ -131,7 +144,7 @@ thing over wasting time hacking a sub-par solution from various topics"""
hours_required = 6
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "testing whether raw odometry + amcl is enough"
description = """
if we can use odometry until we get an amcl reading, then just put in the amcl number over whatever odometry was, and then reset odometry and keep using that until the next update, great, we don't need to use a kalman filter. this todo is to actually check.
@@ -145,7 +158,7 @@ hours_required = 3
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "configuring a kalman filter from robot_localization"
description = """
in case raw odometry + amcl is not enough, a kalman filter has to be used
@@ -161,7 +174,7 @@ purpose = """configuring a kf if necessary"""
hours_required = 6
assignee = "Seba"
-[[project_plan.action_items]]
+[[project_plan.action_items.action_items]]
name = "ros2 whatever map into path planning map"
description = """
we get a bitmap of the map from some ros2 topic. this needs to be starified for our path planning to work.
@@ -185,7 +198,7 @@ hours_required = 0
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "getting map from ros2"
description = """
afaik, the map is build by SLAM from the Nav2 stack. this map is build using all possible sensing shoved into SLAM. it is static and the occupancy is colored black in rviz.
@@ -201,7 +214,7 @@ purpose = """no map no planning, need to get it from nav2"""
hours_required = 4
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "understanding starifiable obstacles"
description = """
need to know what the ros2 map can be made into
@@ -214,7 +227,7 @@ purpose = """no map no planning, need to get it from nav2"""
hours_required = 4
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "starify static map"
description = """
need to know what the ros2 map can be made into
@@ -227,7 +240,7 @@ purpose = """no map no planning, need to get it from nav2"""
hours_required = 3
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "starify dynamic obstacles"
description = """
need to know what the ros2 map can be made into
@@ -240,7 +253,7 @@ purpose = """no map no planning, need to get it from nav2"""
hours_required = 3
assignee = "Seba"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "continuously starify dynamic obstacles"
description = """
need to know what the ros2 map can be made into
@@ -332,6 +345,21 @@ hours_required = 3
assignee = "X"
[[project_plan.action_items]]
+name = "write control code"
+description = """
+everything from doing the math to creating experiments in simulation
+"""
+priority = 1
+is_done = false
+deadline = 2025-02-16
+dependencies = []
+purpose = """
+core of scientific contribution
+"""
+hours_required = 0
+assignee = "Marko"
+
+[[project_plan.action_items.action_items]]
name = "refactor optimal control code"
description = """
it's literally unreadable and changeable. most of it is straight copy-pasting too. while some refactoring is kinda optional, this step most certainly is not.
@@ -347,7 +375,7 @@ hours_required = 0
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "abstract croco optimal control class"
description = """
abstract class to store basic things like the state definition, actuation model etc
@@ -362,7 +390,7 @@ at least 100 less lines of copy-pasted code
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "refactor p2p ocp"
description = """
refactor p2p ocp
@@ -377,7 +405,7 @@ making optimal control fixes possible
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "refactor end-effector path following"
description = """
refactor end-effector path following
@@ -392,24 +420,8 @@ making optimal control fixes possible
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
-name = "rewrite ee construction from past path to be more efficient"
-description = """
-a large past window is needed to store a path long enough to find the point with prefered distance from base. by large i mean large enough that you can't copy-contruct a new np.ndarray from it at every single iteration.
-hence, cache things like path arclength, do math directly on the deque.
-"""
-priority = 1
-is_done = false
-deadline = 2025-02-14
-dependencies = ["refactor p2p ocp"]
-purpose = """
-making optimal control fixes possible
-"""
-hours_required = 3
-assignee = "Marko"
-
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "write end-effector and base path following"
description = """
two versions:
@@ -425,7 +437,7 @@ making optimal control fixes possible
"""
hours_required = 7
-[[project_plan.action_items]]
+[[project_plan.action_items.action_items]]
name = "debug control"
description = """
control stack is broken. could be the control itself, or trajectories' construction from obtained path.
@@ -442,7 +454,7 @@ getting any base and ee path tracking to work
hours_required = 0
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "manage path saving better"
description = """
right now we use a deque to store the path. but this breaks the system if the robot stays in place or moves slowly or not at all - that point eventually fills up the whole queue, the end-effector goes there and everything consequently turns to shit.
@@ -465,7 +477,7 @@ hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "debug past path initialization "
description = """
it's not what it should be - either my math is wrong/there's a bug, and my logic is right, or my logic isn't right, or both
@@ -480,7 +492,7 @@ getting any base and ee path tracking to work
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "ensure either reference doesn't change rapidly"
description = """
getting a suddent change in the path might lead to instability. especially since we are taking dynamics into account.
@@ -501,7 +513,7 @@ getting any base and ee path tracking to work
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "debug base and ee tracking"
description = """
the thing itself might be wrong. you have to test each sub-component to be sure.
@@ -519,7 +531,7 @@ solving the same problem but with a single arm
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "alternative if full-body mpc is not the right tool for the job"
description = """
control just the base with mpc for base tracking.
@@ -535,7 +547,7 @@ half of paper problem
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "dual arm p2p and path following"
description = """
refactor end-effector path following
@@ -550,7 +562,7 @@ half of paper problem
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "cart-pulling (ee and base path following)"
description = """
refactor end-effector path following
@@ -565,7 +577,7 @@ half of paper problem
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "construct map showcasing my prefered setting (make the labyrinth!!!!!!!!)"
description = """
it's the experimtn
@@ -580,7 +592,104 @@ purpose = """
hours_required = 3
assignee = "Marko"
-[[project_plan.action_items]]
+[[project_plan.action_items.action_items.action_items]]
+name = "fixing behavior tree"
+description = """
+ideally the behavior tree todo in smc is done before this, but it's not necessary. what is necessary is to debug the logic errors in the state-selecting conditions, primarily the one deciding when do you grasp handlebar vs push/pull. this requires computer vision to be done because the amount of errors that gives is what determines how this should look.
+possibly more states need to be added, like docking.
+
+the description of the desired behavior tree for the cart pulling system is given below:
+
+make a function that implements the get-cart-to-station behaviour tree.
+ this is a control loop that select what control loop to run based on some
+ logic flags (smaller number = higher priority). simple select loop of the highest priority.
+ for every flag there is a process (node + topic) which determines whether to set it or unset it.
+ - PRIORITY 1: if obstacle flag, then run stopLoop (which is just make all velocities 0). flag on
+ only if there are people around.
+ - PRIORITY 2: if handle_bar flag, run loop to grasp the handle bar. if handle bar is grasped
+ (vishnu's vision sets this flag) then the flag is unset. if not grasped (gripper
+ status (maybe) + vision decides) then set the flag.
+ - PRIORITY 3: pull cart along prescribed path - runs MPC path-following loop. flag on until goal is reached
+ - PRIORITY 4: dock cart. flag on until cart is docked. i guess vision should check that but idk honestly.
+ if this flag is set down, the program is down - shut down.
+ run this on the real thing
+
+UPDATE: prior 1 -> 3, no time, can't be priority 2 anymore, other things are more important. no properly working behaviour tree is needed if we don't use vision.
+"""
+priority = 3
+is_done = false
+deadline = 2025-02-01
+dependencies = []
+purpose = """having functional robotic system, not such 1 controller hopefully working as intended"""
+hours_required = 8
+assignee = "Marko"
+
+[[project_plan.action_items.action_items]]
+name = "improving existing solutions"
+description = """
+some functions were written just to be written as soon as possible, and are quite inefficient. this primarily pertains to using o(n) algorithms instead of o(1) algorithms just recomputing is easier than saving and resuing past compute.
+
+since we're doing mpc, and we know it performs much better with a longer time horizon and if the solver has time to converge, we should try to squeeze as much performance out of it as possible.
+"""
+priority = 2
+is_done = false
+deadline = 2025-02-14
+dependencies = ["smc refactor"]
+purpose = """
+"""
+hours_required = 0
+assignee = "Marko"
+
+[[project_plan.action_items.action_items.action_items]]
+name = "rewrite ee construction from past path to be more efficient"
+description = """
+a large past window is needed to store a path long enough to find the point with prefered distance from base. by large i mean large enough that you can't copy-contruct a new np.ndarray from it at every single iteration.
+
+hence, cache things like path arclength, do math directly on the deque.
+"""
+priority = 1
+is_done = false
+deadline = 2025-02-14
+dependencies = ["refactor p2p ocp"]
+purpose = """
+making optimal control fixes possible
+"""
+hours_required = 3
+assignee = "Marko"
+
+[[project_plan.action_items.action_items.action_items]]
+name = "time scaling on path as decision variable in mpc"
+description = """
+we should not pre-time the path passed to the mpc - it should be a collection of points and that's that. we then of course do need a time scaling on the path. from a math standpoint, it seems the best to have the time scaling of the path as the decision variable. this way, we don't formulate the objective function as distance from knot point to a single pre-selected point on the path, but we really just stay on it as much as possible while traversing it as quickly as possible. not to say that the predefined points are completely unreasonable, but this ios clearly better.
+
+the problem is that then a new objective funtion and it's derivatives need to be defined in crocoddyl. this will take a while to write beucase it's cpp and i haven't done it in a while. the potential upside is that if the derivative is finite-differenced, then getting this done is much easier. it should be investigated whether support for arbitrary functions through finite differencing is a thing in crocoddyl. in any event, this can't be a priority 1 task due to the benefit-time-cost ratio.
+
+UPDATE: prior 2 -> 3, no time, can't be priority 2 anymore, other things are more important. this takes too much time for what it's worth.
+"""
+priority = 3
+is_done = false
+deadline = 2025-02-01
+dependencies = ["tools for trajectory generation in smc"]
+purpose = """having the correct ocp formulation for the whole-body mpc controller for pushing and pulling"""
+hours_required = 20
+assignee = "Marko"
+
+[[project_plan.action_items.action_items.action_items]]
+name = "additional possible modifications of the ocp"
+description = """
+there is a number of possibly interesting ocp formulations, collected here in case the final product is not good enough:
+1) a constraint on the ee poses so that they are in the plane defined by the handlebar
+"""
+priority = 3
+is_done = false
+deadline = 2025-02-01
+dependencies = []
+purpose = """designing a better ocp, testing agaist different options"""
+hours_required = 12
+assignee = "Marko"
+
+
+[[project_plan.action_items.action_items]]
name = "tools for trajectory generation in smc"
description = """
turning a path into a trajectory is bugged. making goals for 2 arms from a single goal is annoying and takes too long. designing SE3 trajectories is cumbersome, and it shouldn't be because it's trivial to do visually or with your hand in the air. there is no reason it should be more than 5x more difficult to input a trajectory on a computer then it is to do it by hand. the same goes for any kind of SE3 targets.
@@ -601,7 +710,7 @@ hours_required = 0
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "just desing the trajectories for the problem"
description = """
debug existing trajectory generation
@@ -614,7 +723,7 @@ purpose = """creating the base of interactive goal/transform/trajectory design""
hours_required = 8
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "extend logging to include saving the whole of save-past-dict"
description = """
the easiest possible way to see what the fuck goes wrong with paths is to save the entire state of the system, and then reproduce it via logging. you already have 90% of this done with log comparison and the saving of the logs. you can just log the entire save-past-window, and not plot it (because it's a matrix at every timestep). save the calculated path as well.
@@ -635,7 +744,7 @@ purpose = """creating the base of interactive goal/transform/trajectory design""
hours_required = 8
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "use drawing feature from drawing demo to draw a path to be followed"
description = """
use drawing feature from drawing demo to draw a path to be followed is very likely the easiest possible way to generate a path that we want to follow. certainly easier than constructing the map.
@@ -650,7 +759,7 @@ purpose = """make test design for just path planning take 10 seconds"""
hours_required = 10
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "interactive meshcat for setting a transformation"
description = """
there is a robot/frame/some object in meshcat. there is a python program waiting for a user command input (keyboard input is fine), so that you don't need to restart the program every time (slow as shit). the thing on screen moves, executing the user's input, ex. left arrow is pressed and the frame goes 0.01m in positive y direction. or you just type in a new value for y, or both. this is probably best done if the cmd-viz is integrated with a notebook/ipython shell/kernel. there also needs to be a button to save (print in correct code format) the designed transform, parameters of the trajectory, whatever it is so that it can be immediately integrated. if the robot's or whatever else state can be stopped while running some control loop, and then switched into the interactive session, that would be the best possible solution (again, having this in some kind of ipython kernel is probably the way to go).
@@ -668,7 +777,7 @@ hours_required = 10
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "reset function"
description = """
reset function is needed to save and reload relevant variables in robotmanager and in visualizer. otherwise you can't go back and try a different transform or parameter.
@@ -685,7 +794,7 @@ hours_required = 2
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "workspace visualization"
description = """
putting workspace objects in makes things easier to verify. this should be goal poses or objects (cart in this case), the table and so on. it should be as minimal as possible. the map visualization should be expanded to render the nav2 bitmap of the map. the abstract things like paths should also be visualized for easier understanding of the control algorithm.
@@ -701,7 +810,7 @@ hours_required = 4
assignee = "Marko"
-[[project_plan.action_items.action_items]]
+[[project_plan.action_items.action_items.action_items]]
name = "path generation parameters interactive tuning"
description = """
a similar type of interface should be developed for path generation parameters, and some of them should be visualized. this primarily concerns the length of the planned path and the safety radius around each point of the path.
@@ -717,71 +826,6 @@ hours_required = 6
assignee = "Marko"
-[[project_plan.action_items]]
-name = "time scaling on path as decision variable in mpc"
-description = """
-we should not pre-time the path passed to the mpc - it should be a collection of points and that's that. we then of course do need a time scaling on the path. from a math standpoint, it seems the best to have the time scaling of the path as the decision variable. this way, we don't formulate the objective function as distance from knot point to a single pre-selected point on the path, but we really just stay on it as much as possible while traversing it as quickly as possible. not to say that the predefined points are completely unreasonable, but this ios clearly better.
-
-the problem is that then a new objective funtion and it's derivatives need to be defined in crocoddyl. this will take a while to write beucase it's cpp and i haven't done it in a while. the potential upside is that if the derivative is finite-differenced, then getting this done is much easier. it should be investigated whether support for arbitrary functions through finite differencing is a thing in crocoddyl. in any event, this can't be a priority 1 task due to the benefit-time-cost ratio.
-
-UPDATE: prior 2 -> 3, no time, can't be priority 2 anymore, other things are more important. this takes too much time for what it's worth.
-"""
-priority = 3
-is_done = false
-deadline = 2025-02-01
-dependencies = ["tools for trajectory generation in smc"]
-purpose = """having the correct ocp formulation for the whole-body mpc controller for pushing and pulling"""
-hours_required = 20
-assignee = "Marko"
-
-
-[[project_plan.action_items]]
-name = "additional possible modifications of the ocp"
-description = """
-there is a number of possibly interesting ocp formulations, collected here in case the final product is not good enough:
-1) a constraint on the ee poses so that they are in the plane defined by the handlebar
-"""
-priority = 3
-is_done = false
-deadline = 2025-02-01
-dependencies = []
-purpose = """designing a better ocp, testing agaist different options"""
-hours_required = 12
-assignee = "Marko"
-
-
-[[project_plan.action_items]]
-name = "fixing behavior tree"
-description = """
-ideally the behavior tree todo in smc is done before this, but it's not necessary. what is necessary is to debug the logic errors in the state-selecting conditions, primarily the one deciding when do you grasp handlebar vs push/pull. this requires computer vision to be done because the amount of errors that gives is what determines how this should look.
-possibly more states need to be added, like docking.
-
-the description of the desired behavior tree for the cart pulling system is given below:
-
-make a function that implements the get-cart-to-station behaviour tree.
- this is a control loop that select what control loop to run based on some
- logic flags (smaller number = higher priority). simple select loop of the highest priority.
- for every flag there is a process (node + topic) which determines whether to set it or unset it.
- - PRIORITY 1: if obstacle flag, then run stopLoop (which is just make all velocities 0). flag on
- only if there are people around.
- - PRIORITY 2: if handle_bar flag, run loop to grasp the handle bar. if handle bar is grasped
- (vishnu's vision sets this flag) then the flag is unset. if not grasped (gripper
- status (maybe) + vision decides) then set the flag.
- - PRIORITY 3: pull cart along prescribed path - runs MPC path-following loop. flag on until goal is reached
- - PRIORITY 4: dock cart. flag on until cart is docked. i guess vision should check that but idk honestly.
- if this flag is set down, the program is down - shut down.
- run this on the real thing
-
-UPDATE: prior 1 -> 3, no time, can't be priority 2 anymore, other things are more important. no properly working behaviour tree is needed if we don't use vision.
-"""
-priority = 3
-is_done = false
-deadline = 2025-02-01
-dependencies = []
-purpose = """having functional robotic system, not such 1 controller hopefully working as intended"""
-hours_required = 8
-assignee = "Marko"
-
[[project_plan.action_items]]
name = "creating tests for whole system performance"
@@ -889,7 +933,7 @@ in text who wrote what is colored.
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
-is_done = false
+is_done = true
dependencies = []
purpose = """this is the product of the project"""
hours_required = 1
@@ -932,7 +976,7 @@ you can see the obstacles in front of you. if you're pushing you need more senso
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
-is_done = false
+is_done = true
dependencies = []
purpose = """nice image explaining modelling"""
hours_required = 4
@@ -948,7 +992,7 @@ NOTE NOTE NOTE: the result of this todo is a lot of smaller todos in this big ac
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
-is_done = false
+is_done = true
dependencies = []
purpose = """part of paper"""
hours_required = 3
@@ -973,12 +1017,10 @@ assignee = "all"
name = "writing introdution"
description = """
we already know what the technology stack is and what we are trying to accomplish. hence it can and should be written down now. this part will focus and guide the code development, and should be done in parallel to it.
-
-ASSIGNED TO SEBA
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
-is_done = false
+is_done = true
dependencies = []
purpose = """part of paper"""
hours_required = 12
@@ -988,8 +1030,6 @@ assignee = "Seba"
name = "writing related work"
description = """
we already know what the technology stack is and what we are trying to accomplish. hence it can and should be written down now. this part will focus and guide the code development, and should be done in parallel to it.
-
-ASSIGNED TO SEBA
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
@@ -1003,8 +1043,6 @@ assignee = "Seba"
name = "writing methodology"
description = """
we already know what the technology stack is and what we are trying to accomplish. hence it can and should be written down now. this part will focus and guide the code development, and should be done in parallel to it.
-
-ASSIGNED TO SEBA
"""
priority = 1
deadline = ["iros paper submission deadline", 2025-03-01]
diff --git a/examples/cartesian_space/test_by_running.sh b/examples/cartesian_space/test_by_running.sh
index cae0cf4e6160f6e3e2dbafb0d0f3f1d05cac170b..61908aca2f3881cb6e3f9504a2ece88e352eb45e 100755
--- a/examples/cartesian_space/test_by_running.sh
+++ b/examples/cartesian_space/test_by_running.sh
@@ -15,13 +15,13 @@ runnable="clik_point_to_point.py --randomly-generate-goal --ik-solver=invKinmQP
echo $runnable
python $runnable
-# damped pseudoinverse single arm whole body mobile
+# damped pseudoinverse whole body single arm mobile
runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
echo $runnable
python $runnable
-# QP whole body mobile
+# QP whole body single arm + mobile
runnable="clik_point_to_point.py --robot=heron --randomly-generate-goal --ik-solver=invKinmQP --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
echo $runnable
python $runnable
@@ -35,3 +35,13 @@ python $runnable
runnable="dual_arm_clik.py --robot=yumi --randomly-generate-goal --ik-solver=invKinmQP --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
echo $runnable
python $runnable
+
+# damped pseudoinverse whole body dual arm + mobile
+runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+echo $runnable
+python $runnable
+
+# QP whole body dual arm + mobile
+runnable="dual_arm_clik.py --robot=myumi --randomly-generate-goal --ik-solver=invKinmQP --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+echo $runnable
+python $runnable
diff --git a/python/setup.py b/python/setup.py
index 2e663a563a75f54e254b8ba91ddb5b1b82fb9e10..ac19f1e9a0e6dd58e0f5466ecc8e1dd364e4c4b0 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -1,6 +1,6 @@
from setuptools import setup, find_packages
setup(name='smc',
- version='0.2',
+ version='0.3',
description='Simple Manipulator Control (SMC) - simplest possible framework for robot control.',
author='Marko Guberina',
url='https://gitlab.control.lth.se/marko-g/ur_simple_control',
@@ -23,14 +23,13 @@ setup(name='smc',
#)
# dependencies:
-# numpy
-# matplotlib
-# qpsolvers
-# quadprog
-# pinocchio
-# meshcat
-# meshcat_shapes
-# crocoddyl
-# albin's path planning repos: starworlds, star_navigation, tunnel_mpc
-# optional for camera: cv2
+# pinocchio - robot math
+# numpy - math
+# matplotlib - plotter
+# meshcat, meshcat_shapes - visualizer
+# optional ik solver: qpsolvers, quadprog, proxsuite
+# optional for opc/mpc: crocoddyl
+# optional for path planning: albin's path planning repos: starworlds, star_navigation, tunnel_mpc
+# optional for visual servoing: cv2
# optional for different ocp: casadi AND pinocchio > 3.0
+# optional for yumi, mobile yumi, and heron: ros2
diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
index 153eddf3844b847030bf736ef07b9bfbd7b112f6..524ce0a06ef5d1b942493572a7cf1156e457f866 100644
--- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
+++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
@@ -213,7 +213,7 @@ def moveLDualArm(
-----------
"""
# TODO: clean this up
- robot._mode = DualArmInterface.control_mode.both
+ robot._mode = DualArmInterface.control_mode.both_arms
ik_solver = getIKSolver(args, robot)
controlLoop = partial(
DualEEP2PCtrlLoopTemplate,
@@ -230,8 +230,8 @@ def moveLDualArm(
"qs": np.zeros(robot.nq),
"dqs": np.zeros(robot.nv),
"dqs_cmd": np.zeros(robot.nv),
- "l_err_norm": np.zeros(6),
- "r_err_norm": np.zeros(6),
+ "l_err_norm": np.zeros(1),
+ "r_err_norm": np.zeros(1),
}
save_past_dict = {}
loop_manager = ControlLoopManager(
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index 4faca5c4e18a744975b77d4365c7a280464d704c..5756795d934a21023d5ea2330a2ba59eca575c77 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -1,5 +1,5 @@
from argparse import Namespace
-from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.robots.abstract_robotmanager import AbstractRobotManager
from smc.multiprocessing.process_manager import ProcessManager
from smc.visualization.plotters import realTimePlotter
diff --git a/python/smc/control/controller_templates/point_to_point.py b/python/smc/control/controller_templates/point_to_point.py
index 828b2178baa86093b3ce9a3b7219551fffe8a43a..67f64b1924cc81ee0dde937c3be63e84e9601dc9 100644
--- a/python/smc/control/controller_templates/point_to_point.py
+++ b/python/smc/control/controller_templates/point_to_point.py
@@ -117,8 +117,8 @@ def DualEEP2PCtrlLoopTemplate(
log_item["qs"] = robot.q
log_item["dqs"] = robot.v
log_item["dqs_cmd"] = v_cmd.reshape((robot.model.nv,))
- log_item["l_err_norm"] = err_vector_left_norm
- log_item["r_err_norm"] = err_vector_right_norm
+ log_item["l_err_norm"] = err_vector_left_norm.reshape((1,))
+ log_item["r_err_norm"] = err_vector_right_norm.reshape((1,))
return breakFlag, save_past_item, log_item
diff --git a/python/smc/robots/abstract_simulated_robotmanager.py b/python/smc/robots/abstract_simulated_robotmanager.py
index 2024e9505623c08282d6fe05c7eb07bb5fd3c569..42eebf15fc8e7fec53ce1736091f2f6758954771 100644
--- a/python/smc/robots/abstract_simulated_robotmanager.py
+++ b/python/smc/robots/abstract_simulated_robotmanager.py
@@ -1,4 +1,4 @@
-from smc.robots.robotmanager_abstract import AbstractRealRobotManager
+from smc.robots.abstract_robotmanager import AbstractRealRobotManager
import pinocchio as pin
import numpy as np
diff --git a/python/smc/robots/grippers/abstract_gripper.py b/python/smc/robots/grippers/abstract_gripper.py
index a261f05aabf9aebbefe80c331a9f4686af1f1d9c..27d5f6e9d2909d935738f8e4fbb1f9ba8726080f 100644
--- a/python/smc/robots/grippers/abstract_gripper.py
+++ b/python/smc/robots/grippers/abstract_gripper.py
@@ -15,8 +15,11 @@ class AbstractGripper(abc.ABC):
There are also the classic expected open(), close(), isGripping() quality-of-life functions.
"""
- def connect(self):
- pass
+ # TODO: make this abstract as well,
+ # but make it possible to have more arguments, or go to grippers and make keyword arguments
+ # for the extra stuff on a case-by-case basis
+ # def connect(self):
+ # pass
@abc.abstractmethod
def move(self, position, speed=None, gripping_force=None):
diff --git a/python/smc/robots/grippers/rs485_robotiq/rs485_robotiq.py b/python/smc/robots/grippers/rs485_robotiq/rs485_robotiq.py
index e40f457dcfc4b389d1d705fea064838ab8468c70..3775ef38c4a3bf7acc81a8c11e2a376f7438bfdc 100644
--- a/python/smc/robots/grippers/rs485_robotiq/rs485_robotiq.py
+++ b/python/smc/robots/grippers/rs485_robotiq/rs485_robotiq.py
@@ -1,4 +1,4 @@
-from ur_simple_control.util.grippers.abstract_gripper import AbstractGripper
+from smc.robots.grippers.abstract_gripper import AbstractGripper
import socket
import time
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index 570ac099a4a42a730c6effd120349e855119c49b..f6f4da313fae979b9161e6f4bf1558e7ab16230d 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -1,3 +1,4 @@
+from smc.robots.abstract_robotmanager import AbstractRealRobotManager
from smc.robots.abstract_simulated_robotmanager import AbstractSimulatedRobotManager
from smc.robots.interfaces.force_torque_sensor_interface import (
ForceTorqueOnSingleArmWrist,
@@ -7,10 +8,20 @@ from smc.robots.interfaces.mobile_base_interface import (
)
from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface
from smc.robots.implementations.ur5e import get_model
+from smc.robots.grippers.robotiq.robotiq_gripper import RobotiqGripper
+from smc.robots.grippers.rs485_robotiq.rs485_robotiq import RobotiqHand
from argparse import Namespace
import numpy as np
import pinocchio as pin
+import time
+
+from importlib.util import find_spec
+
+if find_spec("rtde_control"):
+ from rtde_control import RTDEControlInterface
+ from rtde_receive import RTDEReceiveInterface
+ from rtde_io import RTDEIOInterface
class AbstractHeronRobotManager(
@@ -55,22 +66,19 @@ class SimulatedHeronRobotManager(
self._wrench_bias = np.zeros(6)
-class RealHeronRobotManager(AbstractHeronRobotManager):
+class RealHeronRobotManager(AbstractHeronRobotManager, AbstractRealRobotManager):
def __init__(self, args):
- pass
-
-
-TODO: WRITE KNOW STUFF OUT, MAKE COMMENTS FOR YUYAO ON WHAT TO FILL IN
-
-"""
-TODO: finish
+ if args.debug_prints:
+ print("RealHeronRobotManager init")
self._speed_slider = 1.0 # const
self._rtde_control: RTDEControlInterface
self._rtde_receive: RTDEReceiveInterface
self._rtde_io: RTDEIOInterface
+ raise NotImplementedError
+ # TODO: instantiate topics for reading base position /ekf_something
+ # TODO: instantiate topics for sending base velocity commands /cmd_vel
super().__init__(args)
-
def connectToGripper(self):
if (self.args.gripper == "none") or not self.args.real:
self.gripper = None
@@ -79,9 +87,169 @@ TODO: finish
self.gripper = RobotiqGripper()
self.gripper.connect(self.args.robot_ip, 63352)
self.gripper.activate()
- if self.args.gripper == "onrobot":
- self.gripper = TwoFG()
+ if self.args.gripper == "rs485":
+ self.gripper = RobotiqHand()
+ self.gripper.connect(self.args.robot_ip, 54321)
+ self.gripper.reset()
+ self.gripper.activate()
+ result = self.gripper.wait_activate_complete()
+ if result != 0x31:
+ print("ERROR: can't activate gripper!! - exiting")
+ self.gripper.disconnect()
+ exit()
+
+ def setInitialPose(self):
+ if not self.args.real and self.args.start_from_current_pose:
+ # TODO: read position from localization topic,
+ # put that into _q
+ # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
+ # representation of planar joint state
+ self._q = np.zeros(self.nq)
+ self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
+ self._q[4:] = np.array(self._rtde_receive.getActualQ())
+ if self.args.visualize_manipulator:
+ self.visualizer_manager.sendCommand({"q": self._q})
+ raise NotImplementedError
+ if not self.args.real and not self.args.start_from_current_pose:
+ self._q = pin.randomConfiguration(
+ self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
+ )
+ # pin.RandomConfiguration does not work well for planar joint,
+ # or at least i remember something along those lines being the case
+ theta = np.random.random() * 2 * np.pi - np.pi
+ self._q[2] = np.cos(theta)
+ self._q[3] = np.sin(theta)
+ if self.args.real:
+ # TODO: read position from localization topic,
+ # put that into _q
+ # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
+ # representation of planar joint state
+ self._q = np.zeros(self.nq)
+ self._q[4:] = np.array(self._rtde_receive.getActualQ())
+
+ def connectToRobot(self):
+ # NOTE: you can't connect twice, so you can't have more than one RobotManager per robot.
+ # if this produces errors like "already in use", and it's not already in use,
+ # try just running your new program again. it could be that the socket wasn't given
+ # back to the os even though you've shut off the previous program.
+ print("CONNECTING TO UR5e!")
+ self._rtde_control = RTDEControlInterface(self.args.robot_ip)
+ self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
+ self._rtde_io = RTDEIOInterface(self.args.robot_ip)
+ self._rtde_io.setSpeedSlider(self.args.speed_slider)
+ # NOTE: the force/torque sensor just has large offsets for no reason,
+ # and you need to minus them to have usable readings.
+ # we provide this with calibrateFT
+ self.wrench_offset = self.calibrateFT(self._dt)
+ # TODO:: instantiate topic for reading base position,
+ # i.e. the localization topic
+ raise NotImplementedError
+
+ def setSpeedSlider(self, value):
+ """
+ setSpeedSlider
+ ---------------
+ update in all places
+ NOTE: THIS IS EVIL AND NOTHING WORKS UNLESS IT'S SET TO 1.0!!!
+ USE AT YOUR PERIL IF YOU DON'T KNOW WHAT IT DOES (i don't)
+ """
+ assert value <= 1.0 and value > 0.0
+ if not self.args.pinocchio_only:
+ self._rtde_io.setSpeedSlider(value)
+ self.speed_slider = value
+
+ def _updateQ(self):
+ q = self._rtde_receive.getActualQ()
+ self._q[4:] = np.array(q)
+ # TODO: read position from localization topic,
+ # put that into _q
+ # HAS TO BE [x, y, cos(theta), sin(theta)] due to pinocchio's
+ # representation of planar joint state,
+ # and in the first 4 value of _q
+ raise NotImplementedError
+
+ def _updateV(self):
+ v = self._rtde_receive.getActualQd()
+ # TODO: read base velocity from localization topic(?)
+ # or the IMU. MAKE SURE that the reading (is mapped in)to
+ # the correct frame - ex. on heron the imu is in the camera frame.
+ # put that into _v
+ # HAS TO BE [dot_x, dot_y, dot_theta] and in the first 3 values
+ self._v[3:] = np.array(v)
+
+ def _updateWrench(self):
+ if not self.args.real:
+ self._wrench_base = np.random.random(6)
+ else:
+ # NOTE: UR5e's ft-sensors gives readings in robot's base frame
+ self._wrench_base = (
+ np.array(self._rtde_receive.getActualTCPForce()) - self._wrench_bias
+ )
+ # NOTE: we define the default wrench to be given in the end-effector frame
+ mapping = np.zeros((6, 6))
+ mapping[0:3, 0:3] = self._T_w_e.rotation
+ mapping[3:6, 3:6] = self._T_w_e.rotation
+ self._wrench = mapping.T @ self._wrench_base
+
+ def zeroFtSensor(self):
+ self._rtde_control.zeroFtSensor()
+
+ def sendVelocityCommandToReal(self, v):
+ # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
+ self._rtde_control.speedJ(v[3:], self._acceleration, self._dt)
+ # TODO: send the velocity command to the base by publishing to the
+ # /vel_cmd topic
+ # NOTE: look at sendVelocityCommand in RobotManager,
+ # understand relationship between sendVelocityCommand and sendVelocityCommandToReal
+
+ # NOTE: that sendVelocityCommand is overridden
+ # in SingleArmWholeBodyInterface
+ # (which AbstractHeronRobotManager inherits from) due
+ # to different control modes resulting in different sizes
+ # of qs and vs.
+ # TODO: make sure that this function and they either work together,
+ # or override again if they can't work toherther
+
+ def stopRobot(self):
+ self._rtde_control.speedStop(1)
+ print("sending a stopj as well")
+ self._rtde_control.stopJ(1)
+ print("putting it to freedrive for good measure too")
+ print("stopping via freedrive lel")
+ self._rtde_control.freedriveMode()
+ time.sleep(0.5)
+ self._rtde_control.endFreedriveMode()
+ raise NotImplementedError
+ # TODO: we need to stop be the base as well.
+ # option 1) send zero velocity commands.
+ # but then make sure that it doesn't keep going forward
+ # with a near-zero velocity (this happens on UR5e, that's why
+ # the freedrive is started because it actually stops the arm)
+ # option 2) programaticaly activate the emergency button
+ # option 3) stop just the arm, do nothing for the base, and
+ # clearly document that the robot has to be stopped by manually
+ # pressing the emergency button
+
+ def setFreedrive(self):
+ self._rtde_control.freedriveMode()
+ raise NotImplementedError("freedrive function only written for ur5e")
+ # TODO: if it's possible to manually push the base, great,
+ # put that option here. if not, remove the above error throw,
+ # document that there's no freedrive for the base here
+ # and just put the arm to freedrive mode
+
+ def unSetFreedrive(self):
+ self._rtde_control.endFreedriveMode()
+ raise NotImplementedError("freedrive function only written for ur5e")
+ # TODO: if it's possible to manually push the base, great,
+ # put that option here. if not, remove the above error throw,
+ # document that there's no freedrive for the base here
+ # and just put the arm to freedrive mode
+
+# TODO: see if this is helpful for dealing with ros2 topics,
+# if not then delete
+"""
# TODO: here assert you need to have ros2 installed to run on real heron
# and then set all this bullshit here instead of elsewhere
def set_publisher_vel_base(self, publisher_vel_base):
@@ -110,38 +278,6 @@ TODO: finish
# good to keep because updating is slow otherwise
# it's not correct, but it's more correct than not updating
# self.q = pin.integrate(self.model, self.q, qd * self.dt)
-
- def stopRobot(self):
- print("stopping via freedrive lel")
- self._rtde_control.freedriveMode()
- time.sleep(0.5)
- self._rtde_control.endFreedriveMode()
- raise NotImplementedError(
- "idk how to stop the base lmao. the arm should be the same as for ur5e"
- )
-
- def setFreedrive(self):
- # NOTE: only works for the arm
- self._rtde_control.FreedriveMode()
-
- def unSetFreedrive(self):
- # NOTE: only works for the arm
- self._rtde_control.endFreedriveMode()
-
- def _updateWrench(self):
- if not self.args.real:
- self._wrench_base = np.random.random(6)
- else:
- # NOTE: UR5e's ft-sensors gives readings in robot's base frame
- self._wrench_base = (
- np.array(self._rtde_receive.getActualTCPForce()) - self._wrench_bias
- )
- # NOTE: we define the default wrench to be given in the end-effector frame
- mapping = np.zeros((6, 6))
- mapping[0:3, 0:3] = self._T_w_e.rotation
- mapping[3:6, 3:6] = self._T_w_e.rotation
- self._wrench = mapping.T @ self._wrench_base
-
"""
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index 96f349928e01e5f840217a365c2a7e1248ee837c..b9fd5dce9d5eca6b07c25d7c950039a4dd39092e 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -9,7 +9,7 @@ import pinocchio as pin
from argparse import Namespace
-class AbstractMobileYumiRobotManager(DualArmWholeBodyInterface):
+class AbstractMobileYuMiRobotManager(DualArmWholeBodyInterface):
def __init__(self, args):
if args.debug_prints:
@@ -21,6 +21,8 @@ class AbstractMobileYumiRobotManager(DualArmWholeBodyInterface):
self._l_ee_frame_id = self.model.getFrameId("robl_tool0")
self._r_ee_frame_id = self.model.getFrameId("robr_tool0")
+ self._base_frame_id = self.model.getFrameId("mobile_base")
+ # TODO: CHANGE THIS TO REAL VALUES
self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # const
@@ -39,7 +41,7 @@ class SimulatedMobileYuMiRobotManager(
super().__init__(args)
-class RealMobileYumiRobotManager(AbstractMobileYumiRobotManager):
+class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
def __init__(self, args):
super().__init__(args)
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index ea619bd03edc531b1e94383090ce54723cdb126f..a7ca94570b54b736156575e545fb6bc125414802 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -2,6 +2,7 @@ from smc.robots.interfaces.force_torque_sensor_interface import (
ForceTorqueOnSingleArmWrist,
)
from smc.robots.grippers.robotiq.robotiq_gripper import RobotiqGripper
+from smc.robots.grippers.rs485_robotiq.rs485_robotiq import RobotiqHand
from smc.robots.grippers.on_robot.twofg import TwoFG
from smc.robots.abstract_robotmanager import AbstractRealRobotManager
from smc.robots.abstract_simulated_robotmanager import AbstractSimulatedRobotManager
@@ -79,6 +80,16 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
self.gripper.activate()
if self.args.gripper == "onrobot":
self.gripper = TwoFG()
+ if self.args.gripper == "rs485":
+ self.gripper = RobotiqHand()
+ self.gripper.connect(self.args.robot_ip, 54321)
+ self.gripper.reset()
+ self.gripper.activate()
+ result = self.gripper.wait_activate_complete()
+ if result != 0x31:
+ print("ERROR: can't activate gripper!! - exiting")
+ self.gripper.disconnect()
+ exit()
def setInitialPose(self):
if not self.args.real and self.args.start_from_current_pose:
@@ -94,28 +105,27 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
self._q = np.array(self._rtde_receive.getActualQ())
def connectToRobot(self):
- if self.args.real:
- # NOTE: you can't connect twice, so you can't have more than one RobotManager per robot.
- # if this produces errors like "already in use", and it's not already in use,
- # try just running your new program again. it could be that the socket wasn't given
- # back to the os even though you've shut off the previous program.
- print("CONNECTING TO UR5e!")
- self._rtde_control = RTDEControlInterface(self.args.robot_ip)
- self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
- self._rtde_io = RTDEIOInterface(self.args.robot_ip)
- self._rtde_io.setSpeedSlider(self.args.speed_slider)
- # NOTE: the force/torque sensor just has large offsets for no reason,
- # and you need to minus them to have usable readings.
- # we provide this with calibrateFT
- self.wrench_offset = self.calibrateFT(self._dt)
- else:
- self.wrench_offset = np.zeros(6)
+ # NOTE: you can't connect twice, so you can't have more than one RobotManager per robot.
+ # if this produces errors like "already in use", and it's not already in use,
+ # try just running your new program again. it could be that the socket wasn't given
+ # back to the os even though you've shut off the previous program.
+ print("CONNECTING TO UR5e!")
+ self._rtde_control = RTDEControlInterface(self.args.robot_ip)
+ self._rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
+ self._rtde_io = RTDEIOInterface(self.args.robot_ip)
+ self._rtde_io.setSpeedSlider(self.args.speed_slider)
+ # NOTE: the force/torque sensor just has large offsets for no reason,
+ # and you need to minus them to have usable readings.
+ # we provide this with calibrateFT
+ self.wrench_offset = self.calibrateFT(self._dt)
def setSpeedSlider(self, value):
"""
setSpeedSlider
---------------
update in all places
+ NOTE: THIS IS EVIL AND NOTHING WORKS UNLESS IT'S SET TO 1.0!!!
+ USE AT YOUR PERIL IF YOU DON'T KNOW WHAT IT DOES (i don't)
"""
assert value <= 1.0 and value > 0.0
if not self.args.pinocchio_only:
@@ -163,11 +173,9 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager):
def setFreedrive(self):
self._rtde_control.freedriveMode()
- raise NotImplementedError("freedrive function only written for ur5e")
def unSetFreedrive(self):
self._rtde_control.endFreedriveMode()
- raise NotImplementedError("freedrive function only written for ur5e")
def get_model() -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]:
diff --git a/python/smc/robots/interfaces/whole_body_interface.py b/python/smc/robots/interfaces/whole_body_interface.py
index 4179f0e2519bc80c6110d2636041cfce96781ea0..4ee98111938e21424dcfdc035a297375b35405c5 100644
--- a/python/smc/robots/interfaces/whole_body_interface.py
+++ b/python/smc/robots/interfaces/whole_body_interface.py
@@ -235,18 +235,18 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
# TODO: put back in when python3.12 will be safe to use
# @override
def getJacobian(self) -> np.ndarray:
- J_left = pin.computeFrameJacobian(
+ J_left_with_base = pin.computeFrameJacobian(
self.model, self.data, self._q, self._l_ee_frame_id
- )[:, 3 : (self.model.nv - 3) // 2]
+ )[:, : 3 + (self.model.nv - 3) // 2]
if self._mode == self.control_mode.left_arm_only:
- return J_left
+ return J_left_with_base[:, 3:]
- # the base jacobian can be extracted from either left or right frame -
+ # NOTE: the base jacobian can be extracted from either left or right frame -
# since it's a body jacobian both have to be the same at the base.
# for efficiency of course it would be best to construct it in place,
# but who cares if it runs on time either way.
if self._mode == self.control_mode.base_only:
- return J_left[:, :3]
+ return J_left_with_base[:, :3]
J_right = pin.computeFrameJacobian(
self.model, self.data, self._q, self._r_ee_frame_id
@@ -255,8 +255,11 @@ class DualArmWholeBodyInterface(DualArmInterface, MobileBaseInterface):
return J_right
J_full = np.zeros((12, self.nv))
- J_full[:6, : (self.model.nv - 3) // 2] = J_left
- J_full[6:, (self.model.nv - 3) // 2 :] = J_right
+ J_full[:6, : 3 + (self.model.nv - 3) // 2] = J_left_with_base
+ J_full[6:, 3 + (self.model.nv - 3) // 2 :] = J_right
+ # NOTE: add base for right end-effector
+ # look at note above returning base only jacobian
+ J_full[6:, :3] = J_left_with_base[:, :3]
if self._mode == self.control_mode.both_arms_only:
return J_full[:, 3:]
diff --git a/python/smc/robots/utils.py b/python/smc/robots/utils.py
index 066d96eba15a636211555e9c4416058977053fad..e3875df8bcf96133a376a72f7cc43db1d8664598 100644
--- a/python/smc/robots/utils.py
+++ b/python/smc/robots/utils.py
@@ -1,3 +1,4 @@
+from smc.robots.abstract_robotmanager import AbstractRobotManager
from smc.robots.implementations import *
import numpy as np
@@ -27,7 +28,7 @@ def getMinimalArgParser():
type=str,
help="which robot you're running or simulating",
default="ur5e",
- choices=["ur5e", "heron", "yumi", "mobile_yumi"],
+ choices=["ur5e", "heron", "yumi", "myumi"],
)
parser.add_argument(
"--real",
diff --git a/python/smc/visualization/plotters.py b/python/smc/visualization/plotters.py
index ffe8253d1d09335070fcda90db775a6e468279d3..7a11cdec86e7f309cc4e4594c2357082795b4b37 100644
--- a/python/smc/visualization/plotters.py
+++ b/python/smc/visualization/plotters.py
@@ -156,6 +156,9 @@ def realTimePlotter(args, log_item, queue):
# put in new one
logs_deque[data_key].append(log_item[data_key])
# make it an ndarray (plottable)
+ # TODO: have some asserts here for more informative errors if something
+ # incorrect was passed as the log item.
+ # ideally this is done only once, not every time
logs_ndarrays[data_key] = np.array(logs_deque[data_key])
# now shape == (ROLLING_BUFFER_SIZE, vector_dimension)
for j in range(logs_ndarrays[data_key].shape[1]):