diff --git a/.gitignore b/.gitignore
index 75233b978650f06122fb38f1854a799ee3d8d480..a6402f45c7886ddb5fff43c24cf82c30d61de3f0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,7 +3,9 @@ build/
**/.build
**/build
**__pycache__
+**__pycache__/
**/__pycache__/
+**/__pycache__
.vscode/
**.pickle
**.csv
diff --git a/README.md b/README.md
index 346d9d92eac1f4b58654682bc219cf978915cf2d..ed622b6dcc46f7170143040ee0e8e712da8a2b7a 100644
--- a/README.md
+++ b/README.md
@@ -106,4 +106,3 @@ In the examples directory.
- write all code in c++, make python bindings with pybind. have common documentation and examples
- do the documentation with doxygen so that you get webpages from markdown as well
- create a DMP from multiple trajectories (actually learn a DMP from multiple examples)
-
diff --git a/TODOS b/TODOS
index b7ef7559aa46295c944bd376709f46001644db20..f53b56bfd2a3b41f99a37be02a1fc7d4501d7309 100644
--- a/TODOS
+++ b/TODOS
@@ -1,70 +1,7 @@
-START THE REAL-TIME PLOT CAUSES A DELAY BECAUSE YOU WAIT ON IT
-LET IT START ITSELF, DON'T GIVE THE SUCCESS FLAG BACK!!!!!!!!!
-
-goal 1: starting points for student projects
----------------------------------------------
-1. PUSHING GROUP: some multiprocessing to get camera outputs
- available in the control loop
- (process puts its result on some queue managed by robot_manager,
- and this is popped in the control loop or something.
- think how to do this and then make a skeleton for how to use this).
-2. JENGA GROUP: create a starting point and example with pinocchio.casadi
- optimal control (copy something from tutorial, and use the PD controller
- to track the reference) -> done but not pinocchio.casadi examples
- are not imported to this library -> hopefully they can manage themselves
-
goal 2: challenge
--------------------------------
-mpc for path following
-------------------------
-1.you need to manually make underactuation work in crocoddyl (action model updating etc)
-2. turn mpc from point-to-point to path-following
-3. prepare wrappers for HAL which will make it runnable on the real thing
-
CART HANDLING
----------------
-OPTION 1) FULL DYNAMIC MODELLING --> TOO DIFFICULT TO DO MODELLING-WISE IN SOFTWARE
-----------
-include the cart in the model, formulate OCP with that to move to a point
- 2.2. add a planar joint to one of the arms. it's a big box and that approximates
- the cart reasonably well. alternatively you can define it as an individual thing,
- and then attach the gripper to that. in any event, it's obviously a planar joint,
- and you also have tools to create the (kinematic) model attachment.
- to get the second arm to comply with this, you can create (and update i suppose)
- a rigidBodyConstraint or some kind of contact shenanigans to fix
- it to the free part of the handlebar.
- this opens the question of how do you transmit the forces to the cart.
- an alternative option is to have a separate model for the cart,
- fix contacts for the robot "in the air", compute/extract the contact
- force, do J_cart^T to map that to the cart joint frame,
- and then do rnea with this to get the acceleration. putting some visous
- friction there should be a relatively easy task.
- do note that this complicates life significantly in terms of ACCURATELY
- calculating all the derivatives in the differentialactionmodel you have to make
- for this system.
- in any event, it makes sense to just attach the cart as planar joint
- to the tip of the end-effector. this is what you'll go with on heron,
- so even if the dual-arm case is different you won't do double work.
- when adding a planar object to the gripper, you know you have it
- constricted to the gripper's orientation, so probably maybe you can do it
- the other way for this. if not you need to create the holonomic constraint
- for being at the handlebar. flipping the cart in the roll (or pitch?) axis
- certainly won't happen, but you do have to minimize these forces
- or the yumi will just stop working.
- IT IS ABSOLUTELY NECESSARY TO DISCUSS THIS WITH YIANNIS AND GRAZIANO
- because they certainly know more about this.
- if you have to dodge the mpc and just use pink or whatever
- to get a QP-clik type thing that's fine.
- certainly preferable to not having a working thing.
- yet another alternative is to create contacts on both arms,
- calc/extract wrenches and update them and the cart movement,
- where the dynamic model is blind to the existence of the cart.
- this gets rid of some nice properties, but it might be a decent hack.
- --> start with action model in python, just like the acrobot example
- --> port that to cpp if it's too slow (almost certainly will be lmao)
-4. integrate 3. with actual received path from node running albin's planner
- --> this boils down to just passing the correct references
- DEADLINE: 2024-11-08 -> push to 2024-11-15
5. run that on real heron, tune until it's not broken and ONLY up to that point.
OPTION 2) DISREGARDING THE CART, JUST FOLLOWING END-EFFECTOR REFERENCES (path is defined for the cart)
@@ -75,11 +12,11 @@ OPTION 2) DISREGARDING THE CART, JUST FOLLOWING END-EFFECTOR REFERENCES (path is
if you have the same time-scaling for the path as for the nodes in the ocp,
then you can just do distances of corresponding path point and that node
(for each ee individually)
-ALTERNATIVE TO CREATING A ResidualModelPathPoseTracking
+2. create A ResidualModelPathPoseTracking
could be to set (and update) reference points or residual model
at each running model that comprises the problem.
in fact that might be the correct way to go about it
-2. do a constraint on the ee poses so that they are in the plane defined by the handlebar
+3. do a constraint on the ee poses so that they are in the plane defined by the handlebar
BEHAVIOUR TREE
------------------
@@ -177,3 +114,23 @@ thereby rendering this something publishable in joss
to drake since they also run on iiwa
--> this is easier because it's not broken (panda),
there's no abb egm bullshit, and noone works on that robot
+
+====================================================================================================
+CODEBASE REFACTORING
+====================================================================================================
+NEW guiding principles
+----------------------
+1. non-software people will never actually interact with the intricacies of the library,
+ and hence it is pointless to tailor the library internals for them. instead make it as easy as possible
+ for further development by experts. an expert is someone who knows all aspects of software:
+ object oriented paradigm, templating, decorators, interfaces, multiple abstraction levels,
+ multithreading, IPC, networking, Linux system calls, datastructures and algorithms.
+ no holds barred, just the best code you're capable of writing. you or someone else will probably refactor
+ it again in a few years anyway, if nothing else than for maintenance purposes.
+ of course, you still want it as simple as possible, but now it's ok
+ to use more advanced techniques to get there.
+ for non-experts YOU THE DEVELOPER expose higher-level building blocks and provide
+ a metric ton of examples and documentation. this is not the same documentation as that for the developers,
+ which you also have, but more a user manual.
+2. tests, tests, tests. fixed CI/CD.
+3. aggressive responsibility separation, leading to dependency (and their functionality) compartmentalisation.
diff --git a/development_plan.toml b/development_plan.toml
new file mode 100644
index 0000000000000000000000000000000000000000..e8aee1d12d1f2b79935e127768f402046695d8a4
--- /dev/null
+++ b/development_plan.toml
@@ -0,0 +1,551 @@
+[project_plan]
+name = "smc development plan"
+description = """
+the simple manipulator control library is my phd's flagship software. it will forever retain its essential characteric - get it the robot's urdf, a function to read the robot's state as a vector of number, a function to send the velocity or torque command input, and it will give you the simplest possible API to write your control loop as a few lines of math. on top of that, batteries are included in the sense that it already provides the basic controllers noone wants to be bothered with writing like clik or joint movements.
+
+however, circumstances led to a significat increase of its scope: supporting mobile robots, torque-controlled robots, various optimal control libraries, interactive tuning, dual arm support, whole-body control, ros interface, path planning, multi-robot support, visual servoing and more. literally none of these things follow the core of idea of simplicity and of a limited scope. however, none of them hurt the basic functionality either - they are modular add-ons to the basic scope of simple wrappers around control math. you can use the library as originally intended without interacting or even installing those extensions.
+
+this project became and continues to be collection of everything i've ever done in robotics. but, crucially, since there is a common desing pattern over the whole code-base, these parts can mixed and matched, making all future projects easier because of all the pre-solved parts being easily reusable. so in any event i am benefiting from the library's development.
+
+the key question is whether it can be useful to other people. if so, doing some amount of additional work to polish it makes sense. looking at the wider ros ecosystem, other engineers' opinions of it, and the lack of anything else of the sort out there, it seems like there will be people interested in it - specifically those who want to implement their own control stacks. at that point you need to code anyway, and this library will save you a ton of time because there's so much less overhead than ros. on a personal level, it seems very valuable to develop and manage a large codebase too. so the investment into making it cohesive, well documented and easy to use for newcomers (both for shipping and with examples) will be made.
+
+since i have other things to do, the progress will be very spread out in time and incremental. this makes planning all the more important. the specific sub-projects and feature are outlined in their separate action items. the deadline for the next version is the beggining of the next academic year (2025/2026) so that the new version can be fully employed by the students.
+
+having that said, this developing this library can't never be priority number 1, unless parts of it are necessary for projects which are priority number 1, i.e. specific papers. the given deadline is just for the next full version, after which a plan and deadline for the next version is to be made.
+"""
+priority = 2
+deadline = 2025-09-01
+dependencies = []
+engineering_purpose = """a library for easy and quick robotics prototyping"""
+personal_purpose = """easier time programming robots, flagship software for future employment,
+open source software publication in its own right"""
+# -1 means it should be a sum over sub-items
+hours_required = 0
+is_done = false
+
+[[project_plan.action_items]]
+name = "smc refactor"
+description = """
+during the development of code for the wara challenge, a whole bunch of new features have been introduced in the quickest manner possible with absolute disregard for code quality. the biggest issue is the sequence of if's in some many different places. some of the issues are:
+1) having the same robotmanager class for different robots which makes
+it unreadable - there's some many ifs you forget which function you're looking at.
+2) ifs for two-armed goals, making it difficult to keep track of which goal is which
+3) having or not having a mobile base - if the robot can't move, there is no point in having navigation, and the code itself should know about this - otherwise you need to think for the computer. also, in mobile navigation there are times when you do not want all joint variables to be one big array, which then leads to ugly chopping and slicing over the array. ] this is not something you want to think about when writing navigation algorithms for example - this separation of concerns needs to be available with its own API.
+4) ros stuff is tacked-on and it's ugly as it currently is
+
+on top of solving the issues above, the refactoring should also:
+5) ensure separation of concerns
+6) have a clean cut between smc and albin's path planning - smc should just take the output of the path planner and then not see it again.
+7) process manager should be able to support rtp's on a different computer - necessary for smooth integration of remote GPU for computer vision
+8) cut many random todos scattered throughout the code
+9) args from argparse are getting exceedengly long so there should be an option to load them from a config file which can be overwritten with actual arguments.
+"""
+priority = 1
+deadline = 2025-01-22
+dependencies = []
+purpose = """
+making new features actually usable
+"""
+hours_required = 0
+is_done = false
+
+
+[[project_plan.action_items.action_items]]
+name = "create/fix type-checking workflow"
+description = """
+modern python should have type declarations for static type analysis.
+this has to be used if the library is to be shipped to serious people.
+it also makes all development substantially easier.
+
+to get this to work you need to know how to set it up,
+and then set it up. the components are the following:
+1) find out how to do static type-checking in python (mypy etc)
+2) find out what linting and other concepts are
+3) learn related python concepts: decorators, type overloading and the rest
+4) find out how to make nvim use this automatically
+5) test different software (different lsp server, different lsp configuration and so on)
+
+additionally, the dependencies should be compiled so that they generate
+python stubs so that type-checking actually works, or whatever else is necessary.
+
+TODO: break this up into smaller action items, this is a classic too big,
+unactionable todo that we are trying to avoid by writing stuff out
+"""
+priority = 1
+deadline = 2025-01-22
+dependencies = []
+purpose = """making typing possible - easier code writting and debugging, making library professional-grade"""
+# -1 means it should be a sum over sub-items
+hours_required = 10
+is_done = false
+
+
+[[project_plan.action_items.action_items]]
+name = "introduce typing"
+description = """
+modern python should have type declarations for static type analysis.
+smc does not have these. they are to be put in and leveraged.
+this of course implies that using types works in the nvim setup,
+that the dependencies provide type information etc.
+"""
+priority = 1
+deadline = 2025-01-22
+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.action_items]]
+name = "robotmanager abstract class"
+description = """
+robotmanager needs to be an abstract class, or at least have some abstract functions.
+specific robots then need to provide an implementation of this abstract class.
+having everything in one big class is already untenable.
+robots with ros interfaces should have a ros-less version of functions
+for the pinocchio-only option.
+these can be done via ifs in those functions because 1 if is ok.
+"""
+priority = 1
+deadline = 2025-01-22
+dependencies = []
+purpose = """making codebase usable"""
+# -1 means it should be a sum over sub-items
+hours_required = 4
+is_done = false
+
+
+[[project_plan.action_items.action_items]]
+name = "dual arms feature"
+description = """
+robots with 2 arms need 2 targets. these can simply be 2 separate targets,
+but can also be 2 transformations on 1 target - ex. in cart pulling.
+this seems to best be done by having an (abstract) interface
+which a robot inherits to get access to these functions.
+the solution could also be some other OOP concept
+(it seems like a typical OOP-type problem).
+
+they are also much more likely to have
+self-collisions, so a default controller should be set to that.
+
+the question is how should the controllers be refactored to accomodate
+the 2 targets. maybe having 2 completely separate functions is the way to go,
+it's impossible to say without thinking about it concretely - and
+that's a part of this todo
+"""
+priority = 1
+deadline = 2025-01-22
+dependencies = ["robotmanager abstract class"]
+purpose = """extending existing control for dual arm robots without compromising
+the cleanliness of existing controllers for single arms"""
+hours_required = 8
+is_done = false
+
+
+[[project_plan.action_items.action_items]]
+name = "mobile base feature"
+description = """
+often we want whole-body control from a single controller,
+but not always (ex. just navigating to a task).
+thus the mobile base joint needs to be able to be controlled
+separately. this should not be done by slicing the whole joints vector
+simply because it makes coding more confusing because you then need
+to micromanage the indeces and that detracts from the controller you're writing.
+the todo here is to think of and write the functions to manage the mobile base
+individually, while retaining whole-body control - these need to coexist.
+this could be implemented as an interface that's inherited or
+some similar OOP concept, not sure, deciding is a part of the todo.
+"""
+priority = 1
+deadline = 2025-01-22
+dependencies = ["robotmanager abstract class"]
+purpose = """making it easy, simple, and error-abstinent to only navigate the base"""
+hours_required = 4
+is_done = false
+
+
+[[project_plan.action_items.action_items]]
+name = "velocity vs torque control"
+description = """
+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.
+"""
+priority = 2
+deadline = 2025-09-01
+dependencies = ["robotmanager abstract class"]
+purpose = """paving the way for torque-level control support"""
+hours_required = 3
+is_done = false
+
+
+[[project_plan.action_items.action_items]]
+name = "modularity and dependencies"
+description = """
+this todo boils down to putting "if installed then import"
+everywhere it's logical. there is no point in having optimal control
+libraries installed, and installing them automatically with the library,
+if you don't plan on using them.
+this makes shipping much simpler as well as some packages are, and will be, annoying
+to install.
+the most important part to nicely cut out is ros.
+"""
+priority = 2
+deadline = 2025-01-22
+dependencies = ["robotmanager abstract class"]
+purpose = """easier shipping, enforces modularity and concern separation further,
+easier maintenance. most important for not relying on ros"""
+hours_required = 2
+is_done = false
+
+
+[[project_plan.action_items]]
+name = "writing tests"
+description = """
+write some tests too see that:
+a) various parameter combinations work
+b) controllers converge in situations they should converge in
+c) most basic unit tests on some functions (look at how pinocchio does it)
+d) preferably some of this is runnable on the real robot,
+ or at least sim with --start-from-current pose flag
+e) a speedrun of files in examples on the real robot,
+ just to verify everything works
+
+there is already a bash script that simply runs different examples
+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 = 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 -
+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
+
+
+[[project_plan.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 = "rewrite into cpp"
+description = """
+rewrite the whole library into cpp, expose everything to python
+via pybind. keep the visualizer and the plotter in python since
+that's not performance critical and they already run as separate processes anyway.
+ideally this is something to be done over the summer, but we'll see.
+"""
+priority = 4
+deadline = 2026-09-01
+dependencies = []
+purpose = """easier future work because bigger codebases really like static typing,
+more chill coding because you have a compiler, showing i can write cpp, massive speed boost,
+street cred"""
+# -1 means it should be a sum over sub-items
+hours_required = 80
+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]]
+name = "doing miscellaneous todos scattered throughout the codebase"
+description = """
+a whole bunch of implementation finer points were simply skipped in the interest of time,
+and they need to be done because they were written for a reason.
+however, they are not as essential as the todos which were done and which are here,
+otherwise they'd be solved by now.
+"""
+priority = 3
+deadline = 2026-09-01
+dependencies = []
+purpose = """cleaning up codebase from a small million of random skipped items"""
+# -1 means it should be a sum over sub-items
+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"
+description = """
+all functions need to be documented. some need to be better documented.
+there has to be a uniform documentation standard,
+preferably something from which web or other documentation can be generated.
+(doxygen equivalent in python).
+
+furthermore, the examples need to be documented. this includes explainers
+for the math and the robotics of it all.
+now, if there is a good tutorial/book/video/whatever out there that explains
+something well, then of course link to that and don't invent hot water.
+however, then you still need to find and reference those things,
+and also write a short introduction and connective tissue around.
+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
+deadline = 2026-09-01
+dependencies = []
+purpose = """essential for getting other people to use the library, but not as essential
+as killer features if they aren't written yet"""
+# -1 means it should be a sum over sub-items
+hours_required = 0
+is_done = false
+
+
+[[project_plan.action_items]]
+name = "more controllers"
+description = """
+the more control there is, the better. of course it has to have at least
+one more good reason to be done, it's not it's own purpose.
+"""
+priority = 2
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """increase library power, and making existing feature professional-grade"""
+hours_required = 0
+
+
+[[project_plan.action_items.action_items]]
+name = "fixing QP, putting in adding variants with manip maxing and similar"
+description = """
+the QP formulation is the goated inverse kinematics formulation.
+there are multiple QP solvers out there, and all of the tested ones should be
+made available (in order or performance and preference of course).
+
+for some reason the current version does not work every time and that
+makes absolutely no sense to me. that has to be investigated.
+"""
+priority = 1
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """QP is the best inverse kinematics formulation and should therefore be available"""
+hours_required = 12
+
+
+[[project_plan.action_items.action_items]]
+name = "premade trajectory generation for point-to-point motion"
+description = """
+default point-to-point motion results in weird-looking trajectories if the velocity
+constraints aren't taken into account (and they are only in the qp formulation).
+if you clip them, they lose their shape, again resulting in weird-looking motion.
+
+but generating a path and throwing any reasonable heuristic for timing on that is
+real easy. straight line path, circular path, some bezier curve heuristic thing,
+whatever can be made easily. then you throw a PD controller on that trajectory
+and you're done.
+
+"""
+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"
+description = """
+if this works well for some fun task, refactor albin's dmp's as well.
+bam, nice fun controly control.
+there already is a function to teach a trajectory in freedrive,
+in drawing_on_board there is an example of a taught trajectory
+being passed to a dmp (useful because initial conditions are not the same).
+that's the starting of point for this todo.
+"""
+priority = 4
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """fun and maybe small profit for some tasks and some street cred"""
+hours_required = 20
+
+
+[[project_plan.action_items.action_items]]
+name = "torque-level controllers"
+description = """
+needed for paper which is solved on the torque level.
+also it library should support torque-level controlled robots as well,
+ex. kuka iiwa.
+"""
+priority = 3
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """torque level controlled needed in papers and also to sell the library and me better"""
+hours_required = 30
+
+
+
+[[project_plan.action_items]]
+name = "behavior trees"
+description = """
+for the robot to do more complicated tasks, a behavior tree needs to be formed
+to select different actions depending on the criteria.
+automatons correspond to intelligence of an insect, and that's what gets
+the job done and is understandeable.
+
+the way this works now is you assosiate a number with each control loop you want
+running (there can only be one at one time), have sensors or whatever else
+obtain the state and also associate it with a number - that's the control loop
+you want to run at this time.
+this is even more primitive because you could have different transitions.
+of course you still have an automaton, but one that's easier to model
+for a human.
+
+this is a job for behaviour trees - the robotics way of saying the robot is
+an automaton which does a thing per state, with defined transitions between states.
+there's libraries with GUI tools for setting these trees and other goodies.
+ideally you should just integrate with some of these - writing
+a ton of ifs is untennable if you have more than 4-5 states.
+
+in any event, whatever amount logic is implemented here should be
+a separate part of smc. it should be abstract enough so that it's
+easy to generate new behaviour trees from existing primitives (controlLoops).
+once you have a behavior tree, you stick it into a control loop which
+executes the control loop associated with the current state and that's it.
+the point is this should be cleanly done, not all shoved into one example file.
+"""
+priority = 2
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """behavior tree support that isn't a complete hack unusable in the future.
+it does not need to be great, or even good, but it has to be clean"""
+hours_required = 12
+
+
+[[project_plan.action_items]]
+name = "ready-made function for basic trajectory quality metric calculation"
+description = """
+writing a collection of quality-metric-computing functions
+makes it possible to evaluate how good a controller is
+with something other than eyeballs alone (although that it still the king imo)
+
+some metrics to implement are:
+- smallest manipulability over the trajectory - the higher the better
+- time taken to execute task
+- rms error for path-following controllers
+"""
+priority = 3
+deadline = 2025-09-01
+is_done = false
+dependencies = []
+purpose = """scienfitic analysis of controllers is a good thing to have in the tool box"""
+hours_required = 18
+
+
+#goal 4: more controllers
+#------------------------
+#1. object regrasping with crocoddyl
+# --> to be done in separate repo, planar_in_hand_manipulation,
+# but it should depend on everything here as much as possible
+#2. finish adding all ik algorithms
+# --> try integrating pink for QP inverse kinematics instead
+# of fixing your own code. it's literally the same API from QPsolvers,
+# it's just that this is better documented
+#3. casadi or crocoddyl optimal control with obstacle avoidance (manipulator itself + table/floor is enough)
+# --> outsource as much as possible to student project a, but ofc help w/ infrastructure
+# if necessary
+#4. [hard] adjusting the dmp to get back on the path despite external forces
+# (fix the problem of writing on a non-flat surface/whatever) --> publishable side-project
+# --> give it 2-3 days before robotweek
+#
+#goal 5: more convenience - set gripper payload etc automatically
+#------------------------
+#1. test whether meshcat.display(q) is a blocking or a non-blocking call.
+# if it's a non-blocking call then you don't need to run a separate process to command the visualization
+# # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+# # you already found an API in rtde_control for this, just put it in initialization
+# # under using/not-using gripper parameters
+# # problem: bindings weren't exported to python xd
+# # ALSO NOTE: to use this you need to change the version inclusions in
+# # ur_rtde due to a bug there in the current ur_rtde + robot firmware version
+# # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
+#2. debug comparison log
+#
+#goal 6: panda/yumi
+#----------------
+#finally, do what you promised and put this on another robot,
+#thereby rendering this something publishable in joss
+#
+#1. transfer the library to panda or yumi or both
+# --> better idea to transfer to dual kuka iiwa setup.
+# look how mim people are doing it, but also refer
+# to drake since they also run on iiwa
+# --> this is easier because it's not broken (panda),
+# there's no abb egm bullshit, and noone works on that robot
+#
+#====================================================================================================
+#CODEBASE REFACTORING
+#====================================================================================================
+#NEW guiding principles
+#----------------------
+#1. non-software people will never actually interact with the intricacies of the library,
+# and hence it is pointless to tailor the library internals for them. instead make it as easy as possible
+# for further development by experts. an expert is someone who knows all aspects of software:
+# object oriented paradigm, templating, decorators, interfaces, multiple abstraction levels,
+# multithreading, IPC, networking, Linux system calls, datastructures and algorithms.
+# no holds barred, just the best code you're capable of writing. you or someone else will probably refactor
+# it again in a few years anyway, if nothing else than for maintenance purposes.
+# of course, you still want it as simple as possible, but now it's ok
+# to use more advanced techniques to get there.
+# for non-experts YOU THE DEVELOPER expose higher-level building blocks and provide
+# a metric ton of examples and documentation. this is not the same documentation as that for the developers,
+# which you also have, but more a user manual.
+#2. tests, tests, tests. fixed CI/CD.
+#3. aggressive responsibility separation, leading to dependency (and their functionality) compartmentalisation.
diff --git a/python/examples/camera_no_lag.py b/python/examples/camera_no_lag.py
index 5f3b37d7e84d44efdeef49032fc7f41632f31ed2..83caa748135b89dc9e65de3f13cbc7f3babaa57e 100644
--- a/python/examples/camera_no_lag.py
+++ b/python/examples/camera_no_lag.py
@@ -1,5 +1,10 @@
from ur_simple_control.vision.vision import processCamera
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
+from ur_simple_control.managers import (
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+ ProcessManager,
+)
import argcomplete, argparse
import numpy as np
import time
@@ -9,13 +14,16 @@ from functools import partial
def get_args():
parser = getMinimalArgParser()
- parser.description = 'dummy camera output, but being processed \
- in a different process'
+ parser.description = "dummy camera output, but being processed \
+ in a different process"
argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
-def controlLoopWithCamera(camera_manager : ProcessManager, args, robot : RobotManager, i, past_data):
+
+def controlLoopWithCamera(
+ camera_manager: ProcessManager, args, robot: RobotManager, i, past_data
+):
"""
controlLoopWithCamera
-----------------------------
@@ -27,34 +35,32 @@ def controlLoopWithCamera(camera_manager : ProcessManager, args, robot : RobotMa
q = robot.getQ()
camera_output = camera_manager.getData()
- #print(camera_output)
+ # print(camera_output)
qd_cmd = np.zeros(robot.model.nv)
robot.sendQd(qd_cmd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- log_item['camera_output'] = np.array([camera_output['x'],
- camera_output['y']])
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["camera_output"] = np.array([camera_output["x"], camera_output["y"]])
return breakFlag, save_past_dict, log_item
-if __name__ == "__main__":
+
+if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
# cv2 camera device 0
device = 0
side_function = partial(processCamera, device)
- init_value = {"x" : np.random.randint(0, 10),
- "y" : np.random.randint(0, 10)}
+ init_value = {"x": np.random.randint(0, 10), "y": np.random.randint(0, 10)}
camera_manager = ProcessManager(args, side_function, {}, 1, init_value=init_value)
-
log_item = {}
- log_item['qs'] = np.zeros((robot.model.nq,))
- log_item['dqs'] = np.zeros((robot.model.nv,))
- log_item['camera_output'] = np.zeros(2)
+ log_item["qs"] = np.zeros((robot.model.nq,))
+ log_item["dqs"] = np.zeros((robot.model.nv,))
+ log_item["camera_output"] = np.zeros(2)
controlLoop = partial(controlLoopWithCamera, camera_manager, args, robot)
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
@@ -67,7 +73,7 @@ if __name__ == "__main__":
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
-
+
if args.save_log:
robot.log_manager.saveLog()
robot.log_manager.plotAllControlLoops()
diff --git a/python/examples/clik.py b/python/examples/clik.py
index e58580b571226ccefe078663ce9ea6a6ba59de04..e6d2587742d957b4dbd1766337a55bfaa494bd00 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -1,37 +1,46 @@
# PYTHON_ARGCOMPLETE_OK
import numpy as np
-import time
import pinocchio as pin
import argcomplete, argparse
-from functools import partial
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, moveLDualArm
+from ur_simple_control.managers import getMinimalArgParser, RobotManager
+from ur_simple_control.clik.clik import (
+ getClikArgs,
+ getClikController,
+ controlLoopClik,
+ moveL,
+ compliantMoveL,
+ moveLDualArm,
+)
from ur_simple_control.util.define_random_goal import getRandomlyGeneratedGoal
def get_args():
parser = getMinimalArgParser()
- parser.description = 'Run closed loop inverse kinematics \
- of various kinds. Make sure you know what the goal is before you run!'
+ parser.description = "Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!"
parser = getClikArgs(parser)
- parser.add_argument('--randomly-generate-goal', action=argparse.BooleanOptionalAction,
- default=False, help="if true, rand generate a goal, if false you type it in via text input")
+ parser.add_argument(
+ "--randomly-generate-goal",
+ action=argparse.BooleanOptionalAction,
+ default=False,
+ help="if true, rand generate a goal, if false you type it in via text input",
+ )
argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
-if __name__ == "__main__":
+
+if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
if args.randomly_generate_goal:
Mgoal = getRandomlyGeneratedGoal(args)
if args.visualize_manipulator:
- robot.visualizer_manager.sendCommand(
- {"Mgoal" : Mgoal})
+ robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
else:
Mgoal = robot.defineGoalPointCLI()
Mgoal.rotation = np.eye(3)
- #compliantMoveL(args, robot, Mgoal)
+ # compliantMoveL(args, robot, Mgoal)
if robot.robot_name != "yumi":
moveL(args, robot, Mgoal)
else:
@@ -45,8 +54,7 @@ if __name__ == "__main__":
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
-
+
if args.save_log:
robot.log_manager.saveLog()
- #loop_manager.stopHandler(None, None)
-
+ # loop_manager.stopHandler(None, None)
diff --git a/python/examples/force_control_test.py b/python/examples/force_control_test.py
index 949e06a9583c2d554bc31832dfab04a08949804d..f458bbdaa7f0645fb01bef2ad9203edfa725a27a 100644
--- a/python/examples/force_control_test.py
+++ b/python/examples/force_control_test.py
@@ -1,18 +1,22 @@
import pinocchio as pin
import numpy as np
-import time
-import argparse
from functools import partial
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.managers import (
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+)
+
def get_args():
parser = getMinimalArgParser()
- parser.description = 'force control example'
+ parser.description = "force control example"
# add more arguments here from different Simple Manipulator Control modules
args = parser.parse_args()
return args
-def controlLoopForceEx(args, robot : RobotManager, T_w_e_init, i, past_data):
+
+def controlLoopForceEx(args, robot: RobotManager, T_w_e_init, i, past_data):
"""
controlLoop
-----------------------------
@@ -25,8 +29,10 @@ def controlLoopForceEx(args, robot : RobotManager, T_w_e_init, i, past_data):
q = robot.getQ()
T_w_e = robot.getT_w_e()
wrench = robot.getWrench()
- save_past_dict['wrench'] = wrench.copy()
- wrench = 0.5 * wrench + (1 - 0.5) * np.average(np.array(past_data['wrench']), axis=0)
+ save_past_dict["wrench"] = wrench.copy()
+ wrench = 0.5 * wrench + (1 - 0.5) * np.average(
+ np.array(past_data["wrench"]), axis=0
+ )
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
Kp = 1.0
@@ -37,22 +43,27 @@ def controlLoopForceEx(args, robot : RobotManager, T_w_e_init, i, past_data):
qd_cmd = Kp * J.T @ error_v + J.T @ (wrench_ref - wrench)
robot.sendQd(qd_cmd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- log_item['wrench'] = wrench
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["wrench"] = wrench
return breakFlag, save_past_dict, log_item
-if __name__ == "__main__":
+
+if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
q_init = robot.getQ()
T_w_e_init = robot.getT_w_e()
controlLoop = partial(controlLoopForceEx, args, robot, T_w_e_init)
- log_item = {'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv),
- 'wrench' : np.zeros(6)}
- loop_manager = ControlLoopManager(robot, controlLoop, args, {'wrench' : np.zeros(6)} , log_item)
+ log_item = {
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "wrench": np.zeros(6),
+ }
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, {"wrench": np.zeros(6)}, log_item
+ )
loop_manager.run()
# get expected behaviour here (library can't know what the end is - you have to do this here)
@@ -64,7 +75,7 @@ if __name__ == "__main__":
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
-
+
if args.save_log:
robot.log_manager.saveLog()
- #loop_manager.stopHandler(None, None)
+ # loop_manager.stopHandler(None, None)
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 6e4bc966156f0327a12f9f3ddd821ffbba402ba1..50e963a686145aea75acfda8b60b01d8e59004f6 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,12 +1,12 @@
-0.00000,0.89672,-1.14483,-1.60108,-1.19515,2.08894,-0.46310
-0.90909,0.86014,-1.15888,-1.59376,-1.20231,2.11354,-0.49311
-1.81818,0.78343,-1.22114,-1.54642,-1.22208,2.16489,-0.55978
-2.72727,0.73242,-1.29426,-1.47359,-1.24837,2.19812,-0.60696
-3.63636,0.72427,-1.30838,-1.45836,-1.25398,2.20334,-0.61473
-4.54545,0.71705,-1.32065,-1.44531,-1.25883,2.20795,-0.62166
-5.45455,0.71180,-1.32990,-1.43534,-1.26254,2.21129,-0.62674
-6.36364,0.70894,-1.33521,-1.42951,-1.26470,2.21309,-0.62951
-7.27273,0.70580,-1.34125,-1.42281,-1.26719,2.21509,-0.63258
-8.18182,0.70304,-1.34727,-1.41584,-1.26974,2.21683,-0.63528
-9.09091,0.69707,-1.36317,-1.39643,-1.27676,2.22059,-0.64116
-10.00000,0.69705,-1.36322,-1.39637,-1.27678,2.22060,-0.64118
+0.00000,0.89399,-1.14443,-1.59870,-1.19904,2.09084,-0.46538
+0.90909,0.85737,-1.15854,-1.59128,-1.20634,2.11545,-0.49549
+1.81818,0.78072,-1.22087,-1.54384,-1.22630,2.16669,-0.56226
+2.72727,0.72981,-1.29410,-1.47091,-1.25266,2.19980,-0.60946
+3.63636,0.72173,-1.30816,-1.45575,-1.25825,2.20497,-0.61719
+4.54545,0.71453,-1.32044,-1.44268,-1.26311,2.20956,-0.62412
+5.45455,0.70929,-1.32971,-1.43271,-1.26682,2.21288,-0.62919
+6.36364,0.70645,-1.33502,-1.42687,-1.26899,2.21468,-0.63196
+7.27273,0.70331,-1.34106,-1.42016,-1.27147,2.21666,-0.63503
+8.18182,0.70057,-1.34709,-1.41319,-1.27403,2.21839,-0.63772
+9.09091,0.69463,-1.36301,-1.39376,-1.28104,2.22213,-0.64359
+10.00000,0.69461,-1.36306,-1.39370,-1.28106,2.22214,-0.64361
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index acc0302fedf593c360f8cf0382af990a27288650..8897b55369ebe6793f41236b28ad1f3d49d301be 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -7,26 +7,47 @@ import time
from functools import partial
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
-from ur_simple_control.dmp.dmp import DMP, NoTC,TCVelAccConstrained
-from ur_simple_control.clik.clik import getClikArgs, getClikController, moveL, moveUntilContact
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.dmp.dmp import DMP, NoTC, TCVelAccConstrained
+from ur_simple_control.clik.clik import (
+ getClikArgs,
+ getClikController,
+ moveL,
+ moveUntilContact,
+)
+from ur_simple_control.managers import (
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+)
from ur_simple_control.basics.basics import moveJPI
+
def getArgs():
parser = getMinimalArgParser()
parser = getClikArgs(parser)
- parser.add_argument('--kp', type=float, \
- help="proportial control constant for position errors", \
- default=1.0)
- parser.add_argument('--kv', type=float, \
- help="damping in impedance control", \
- default=0.001)
- parser.add_argument('--cartesian-space-impedance', action=argparse.BooleanOptionalAction, \
- help="is the impedance computed and added in cartesian or in joint space", default=False)
- parser.add_argument('--z-only', action=argparse.BooleanOptionalAction, \
- help="whether you have general impedance or just ee z axis", default=False)
-
-# argcomplete.autocomplete(parser)
+ parser.add_argument(
+ "--kp",
+ type=float,
+ help="proportial control constant for position errors",
+ default=1.0,
+ )
+ parser.add_argument(
+ "--kv", type=float, help="damping in impedance control", default=0.001
+ )
+ parser.add_argument(
+ "--cartesian-space-impedance",
+ action=argparse.BooleanOptionalAction,
+ help="is the impedance computed and added in cartesian or in joint space",
+ default=False,
+ )
+ parser.add_argument(
+ "--z-only",
+ action=argparse.BooleanOptionalAction,
+ help="whether you have general impedance or just ee z axis",
+ default=False,
+ )
+
+ # argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
@@ -35,8 +56,11 @@ def getArgs():
def controller():
pass
+
# control loop to be passed to ControlLoopManager
-def controlLoopPointImpedance(args, q_init, controller, robot : RobotManager, i, past_data):
+def controlLoopPointImpedance(
+ args, q_init, controller, robot: RobotManager, i, past_data
+):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -44,36 +68,41 @@ def controlLoopPointImpedance(args, q_init, controller, robot : RobotManager, i,
q = robot.getQ()
Mtool = robot.getT_w_e()
wrench = robot.getWrench()
- log_item['wrench_raw'] = wrench.reshape((6,))
+ log_item["wrench_raw"] = wrench.reshape((6,))
# deepcopy for good coding practise (and correctness here)
- save_past_dict['wrench'] = copy.deepcopy(wrench)
+ save_past_dict["wrench"] = copy.deepcopy(wrench)
# rolling average
- #wrench = np.average(np.array(past_data['wrench']), axis=0)
+ # wrench = np.average(np.array(past_data['wrench']), axis=0)
# first-order low pass filtering instead
# beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
- #wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
+ # wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(
+ np.array(past_data["wrench"]), axis=0
+ )
if not args.z_only:
Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
else:
Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
- #Z = np.diag(np.ones(6))
+ # Z = np.diag(np.ones(6))
wrench = Z @ wrench
-
+
# this jacobian might be wrong
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
- dq = robot.getQd()[:6].reshape((6,1))
- # get joint
+ dq = robot.getQd()[:6].reshape((6, 1))
+ # get joint
tau = J.T @ wrench
-# if i % 25 == 0:
-# print(*tau.round(1))
- tau = tau[:6].reshape((6,1))
+ # if i % 25 == 0:
+ # print(*tau.round(1))
+ tau = tau[:6].reshape((6, 1))
# compute control law:
- # - feedback the position
+ # - feedback the position
# kv is not needed if we're running velocity control
- vel_cmd = args.kp * (q_init[:6].reshape((6,1)) - q[:6].reshape((6,1))) + args.alpha * tau
- #vel_cmd = np.zeros(6)
+ vel_cmd = (
+ args.kp * (q_init[:6].reshape((6, 1)) - q[:6].reshape((6, 1)))
+ + args.alpha * tau
+ )
+ # vel_cmd = np.zeros(6)
robot.sendQd(vel_cmd)
# immediatelly stop if something weird happened (some non-convergence)
@@ -82,15 +111,16 @@ def controlLoopPointImpedance(args, q_init, controller, robot : RobotManager, i,
# log what you said you'd log
# TODO fix the q6 situation (hide this)
- log_item['qs'] = q[:6].reshape((6,))
- log_item['dqs'] = dq.reshape((6,))
- log_item['wrench_used'] = wrench.reshape((6,))
+ log_item["qs"] = q[:6].reshape((6,))
+ log_item["dqs"] = dq.reshape((6,))
+ log_item["wrench_used"] = wrench.reshape((6,))
return breakFlag, save_past_dict, log_item
-
-def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot : RobotManager, i, past_data):
+def controlLoopCartesianPointImpedance(
+ args, Mtool_init, clik_controller, robot: RobotManager, i, past_data
+):
breakFlag = False
# TODO rename this into something less confusing
save_past_dict = {}
@@ -98,26 +128,28 @@ def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot
q = robot.getQ()
Mtool = robot.getT_w_e()
wrench = robot.getWrench()
- log_item['wrench_raw'] = wrench.reshape((6,))
- save_past_dict['wrench'] = copy.deepcopy(wrench)
- #wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
+ log_item["wrench_raw"] = wrench.reshape((6,))
+ save_past_dict["wrench"] = copy.deepcopy(wrench)
+ # wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(
+ np.array(past_data["wrench"]), axis=0
+ )
# good generic values
- #Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
+ # Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
# but let's stick to the default for now
if not args.z_only:
Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
else:
Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
- #Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
- #Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
+ # Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
+ # Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
wrench = Z @ wrench
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
SEerror = Mtool.actInv(Mtool_init)
- err_vector = pin.log6(SEerror).vector
+ err_vector = pin.log6(SEerror).vector
v_cartesian_body_cmd = args.kp * err_vector + args.alpha * wrench
vel_cmd = clik_controller(J, v_cartesian_body_cmd)
@@ -127,49 +159,55 @@ def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot
if np.isnan(vel_cmd[0]):
breakFlag = True
- dq = robot.getQd()[:6].reshape((6,1))
+ dq = robot.getQd()[:6].reshape((6, 1))
# log what you said you'd log
# TODO fix the q6 situation (hide this)
- log_item['qs'] = q[:6].reshape((6,))
- log_item['dqs'] = dq.reshape((6,))
- log_item['wrench_used'] = wrench.reshape((6,))
+ log_item["qs"] = q[:6].reshape((6,))
+ log_item["dqs"] = dq.reshape((6,))
+ log_item["wrench_used"] = wrench.reshape((6,))
return breakFlag, save_past_dict, log_item
if __name__ == "__main__":
args = getArgs()
- clikController = getClikController(args)
robot = RobotManager(args)
-
+ clikController = getClikController(args, robot)
+
# TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
- # you already found an API in rtde_control for this, just put it in initialization
+ # you already found an API in rtde_control for this, just put it in initialization
# under using/not-using gripper parameters
# ALSO NOTE: to use this you need to change the version inclusions in
- # ur_rtde due to a bug there in the current ur_rtde + robot firmware version
+ # ur_rtde due to a bug there in the current ur_rtde + robot firmware version
# (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
- # here you give what you're saving in the rolling past window
+ # here you give what you're saving in the rolling past window
# it's initial value.
# controlLoopManager will populate the queue with these initial values
save_past_dict = {
- 'wrench' : np.zeros(6),
- }
+ "wrench": np.zeros(6),
+ }
# here you give it it's initial value
log_item = {
- 'qs' : np.zeros(robot.n_arm_joints),
- 'dqs' : np.zeros(robot.n_arm_joints),
- 'wrench_raw' : np.zeros(6),
- 'wrench_used' : np.zeros(6),
- }
+ "qs": np.zeros(robot.n_arm_joints),
+ "dqs": np.zeros(robot.n_arm_joints),
+ "wrench_raw": np.zeros(6),
+ "wrench_used": np.zeros(6),
+ }
q_init = robot.getQ()
Mtool_init = robot.getT_w_e()
if not args.cartesian_space_impedance:
- controlLoop = partial(controlLoopPointImpedance, args, q_init, controller, robot)
+ controlLoop = partial(
+ controlLoopPointImpedance, args, q_init, controller, robot
+ )
else:
- controlLoop = partial(controlLoopCartesianPointImpedance, args, Mtool_init, clikController, robot)
+ controlLoop = partial(
+ controlLoopCartesianPointImpedance, args, Mtool_init, clikController, robot
+ )
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
loop_manager.run()
if not args.pinocchio_only:
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index f483a01458224dd004f83b838d10cbd14cccf706..69c68cad978f4dfb5a8e0f4a8ea599a674315448 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-311.pyc
index 46754c908e5bf6f1099bbea41f50042c6e03f530..78c3062d42ad1d30bc12d1624dcf1fd749abd2d0 100644
Binary files a/python/ur_simple_control/basics/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/basics/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-311.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-311.pyc
index bf9003e76ee7a4f45b1083add2ad0f60642967f1..3100ec7ba0ff30ee9bd30b615348d3ea9b94afcf 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-311.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-311.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 1b140aeaa8f61a415e26bda0975b9c1a73cfb34d..b4e74ffda1c31794ba71304811ce48f8b262a8d2 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -9,8 +9,13 @@ from qpsolvers import solve_qp
import argparse
import importlib
import proxsuite
-if importlib.util.find_spec('shapely'):
- from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
+
+if importlib.util.find_spec("shapely"):
+ from ur_simple_control.path_generation.planner import (
+ path2D_to_timed_SE3,
+ pathPointFromPathParam,
+ )
+
def getClikArgs(parser):
"""
@@ -20,51 +25,84 @@ def getClikArgs(parser):
This way the rest of the code is clean.
Force filtering is considered a core part of these control loops
because it's not necessarily the same as elsewhere.
- If you want to know what these various arguments do, just grep
+ If you want to know what these various arguments do, just grep
though the code to find them (but replace '-' with '_' in multi-word arguments).
All the sane defaults are here, and you can change any number of them as an argument
when running your program. If you want to change a drastic amount of them, and
not have to run a super long commands, just copy-paste this function and put your
own parameters as default ones.
"""
- #parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
+ # parser = argparse.ArgumentParser(description='Run closed loop inverse kinematics \
# of various kinds. Make sure you know what the goal is before you run!',
# formatter_class=argparse.ArgumentDefaultsHelpFormatter)
####################
# clik arguments #
####################
- parser.add_argument('--goal-error', type=float, \
- help="the final position error you are happy with", default=1e-2)
- parser.add_argument('--tikhonov-damp', type=float, \
- help="damping scalar in tikhonov regularization", default=1e-3)
+ parser.add_argument(
+ "--goal-error",
+ type=float,
+ help="the final position error you are happy with",
+ default=1e-2,
+ )
+ parser.add_argument(
+ "--tikhonov-damp",
+ type=float,
+ help="damping scalar in tikhonov regularization",
+ default=1e-3,
+ )
# TODO add the rest
- parser.add_argument('--clik-controller', type=str, \
- help="select which click algorithm you want", \
- default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose', 'invKinmQP', 'QPproxsuite'])
+ parser.add_argument(
+ "--clik-controller",
+ type=str,
+ help="select which click algorithm you want",
+ default="dampedPseudoinverse",
+ choices=[
+ "dampedPseudoinverse",
+ "jacobianTranspose",
+ "invKinmQP",
+ "QPproxsuite",
+ ],
+ )
###########################################
# force sensing and feedback parameters #
###########################################
- parser.add_argument('--alpha', type=float, \
- help="force feedback proportional coefficient", \
- default=0.01)
- parser.add_argument('--beta', type=float, \
- help="low-pass filter beta parameter", \
- default=0.01)
+ parser.add_argument(
+ "--alpha",
+ type=float,
+ help="force feedback proportional coefficient",
+ default=0.01,
+ )
+ parser.add_argument(
+ "--beta", type=float, help="low-pass filter beta parameter", default=0.01
+ )
###############################
# path following parameters #
###############################
- parser.add_argument('--max-init-clik-iterations', type=int, \
- help="number of max clik iterations to get to the first point", default=10000)
- parser.add_argument('--max-running-clik-iterations', type=int, \
- help="number of max clik iterations between path points", default=1000)
- parser.add_argument('--viz-test-path', action=argparse.BooleanOptionalAction, \
- help="number of max clik iterations between path points", default=False)
-
+ parser.add_argument(
+ "--max-init-clik-iterations",
+ type=int,
+ help="number of max clik iterations to get to the first point",
+ default=10000,
+ )
+ parser.add_argument(
+ "--max-running-clik-iterations",
+ type=int,
+ help="number of max clik iterations between path points",
+ default=1000,
+ )
+ parser.add_argument(
+ "--viz-test-path",
+ action=argparse.BooleanOptionalAction,
+ help="number of max clik iterations between path points",
+ default=False,
+ )
+
return parser
+
#######################################################################
# controllers #
#######################################################################
@@ -81,14 +119,21 @@ the actual control loop after you've put it in getClikController,
which constructs the controller from an argument to the file.
"""
+
def dampedPseudoinverse(tikhonov_damp, J, err_vector):
- qd = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * tikhonov_damp) @ err_vector
+ qd = (
+ J.T
+ @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * tikhonov_damp)
+ @ err_vector
+ )
return qd
+
def jacobianTranspose(J, err_vector):
qd = J.T @ err_vector
return qd
+
# TODO: put something into q of the QP
# also, put in lb and ub
# this one is with qpsolvers
@@ -97,17 +142,17 @@ def invKinmQP(J, err_vector, lb=None, ub=None, past_qd=None):
invKinmQP
---------
generic QP:
- minimize 1/2 x^T P x + q^T x
+ minimize 1/2 x^T P x + q^T x
subject to
- G x \leq h
- A x = b
+ G x \leq h
+ A x = b
lb <= x <= ub
inverse kinematics QP:
- minimize 1/2 qd^T P qd
+ minimize 1/2 qd^T P qd
+ q^T qd (optional secondary objective)
subject to
G qd \leq h (optional)
- J qd = b (mandatory)
+ J qd = b (mandatory)
lb <= qd <= ub (optional)
"""
P = np.eye(J.shape[1], dtype="double")
@@ -118,49 +163,50 @@ def invKinmQP(J, err_vector, lb=None, ub=None, past_qd=None):
b = err_vector
A = J
# TODO: you probably want limits here
- #lb = None
- #ub = None
- #lb *= 20
- #ub *= 20
+ # lb = None
+ # ub = None
+ # lb *= 20
+ # ub *= 20
h = None
# (n_vars, n_eq_constraints, n_ineq_constraints)
- #qp.init(H, g, A, b, C, l, u)
- #print(J.shape)
- #print(q.shape)
- #print(A.shape)
- #print(b.shape)
-# NOTE: you want to pass the previous solver, not recreate it every time
-######################
-# solve it
- #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
- #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=True, initvals=np.ones(len(lb)))
- #if not (past_qd is None):
+ # qp.init(H, g, A, b, C, l, u)
+ # print(J.shape)
+ # print(q.shape)
+ # print(A.shape)
+ # print(b.shape)
+ # NOTE: you want to pass the previous solver, not recreate it every time
+ ######################
+ # solve it
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=True, initvals=np.ones(len(lb)))
+ # if not (past_qd is None):
# qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=False, initvals=past_qd)
- #else:
+ # else:
# qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=False, initvals=J.T@err_vector)
- #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=True, initvals=0.01*J.T@err_vector)
- #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False, initvals=0.01*J.T@err_vector)
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp", verbose=True, initvals=0.01*J.T@err_vector)
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False, initvals=0.01*J.T@err_vector)
qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog", verbose=False)
- #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp")
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="proxqp")
return qd
def QPproxsuite(qp, lb, ub, J, err_vector):
# proxsuite does lb <= Cx <= ub
qp.settings.initial_guess = (
- proxsuite.proxqp.InitialGuess.WARM_START_WITH_PREVIOUS_RESULT
-)
- #qp.update(g=q, A=A, b=h, l=lb, u=ub)
+ proxsuite.proxqp.InitialGuess.WARM_START_WITH_PREVIOUS_RESULT
+ )
+ # qp.update(g=q, A=A, b=h, l=lb, u=ub)
qp.update(A=J, b=err_vector)
qp.solve()
qd = qp.results.x
if qp.results.info.status == proxsuite.proxqp.PROXQP_PRIMAL_INFEASIBLE:
- #if np.abs(qp.results.info.duality_gap) > 0.1:
+ # if np.abs(qp.results.info.duality_gap) > 0.1:
print("didn't solve shit")
qd = None
return qd
+
# TODO: calculate nice q (in QP) as the secondary objective
# this requires getting the forward kinematics hessian,
# a.k.a jacobian derivative w.r.t. joint positions dJ/dq .
@@ -218,23 +264,22 @@ Tgn = Tdiffq(costManipulability.calc,q)
"""
-
def QPManipMax(J, err_vector, lb=None, ub=None):
"""
invKinmQP
---------
generic QP:
- minimize 1/2 x^T P x + q^T x
+ minimize 1/2 x^T P x + q^T x
subject to
- G x \leq h
- A x = b
+ G x \leq h
+ A x = b
lb <= x <= ub
inverse kinematics QP:
- minimize 1/2 qd^T P qd
+ minimize 1/2 qd^T P qd
+ q^T qd (optional secondary objective)
subject to
G qd \leq h (optional)
- J qd = b (mandatory)
+ J qd = b (mandatory)
lb <= qd <= ub (optional)
"""
P = np.eye(J.shape[1], dtype="double")
@@ -245,13 +290,14 @@ def QPManipMax(J, err_vector, lb=None, ub=None):
b = err_vector
A = J
# TODO: you probably want limits here
- #lb = None
- #ub = None
+ # lb = None
+ # ub = None
h = None
- #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
+ # qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
return qd
+
def getClikController(args, robot):
"""
getClikController
@@ -268,17 +314,17 @@ def getClikController(args, robot):
if args.clik_controller == "jacobianTranspose":
return jacobianTranspose
# TODO implement and add in the rest
- #if controller_name == "invKinmQPSingAvoidE_kI":
+ # if controller_name == "invKinmQPSingAvoidE_kI":
# return invKinmQPSingAvoidE_kI
- #if controller_name == "invKinm_PseudoInv":
+ # if controller_name == "invKinm_PseudoInv":
# return invKinm_PseudoInv
- #if controller_name == "invKinm_PseudoInv_half":
+ # if controller_name == "invKinm_PseudoInv_half":
# return invKinm_PseudoInv_half
if args.clik_controller == "invKinmQP":
- lb = -1 * np.array(robot.model.velocityLimit, dtype='double')
+ lb = -1 * np.array(robot.model.velocityLimit, dtype="double")
# we do additional clipping
lb = np.clip(lb, -1 * robot.max_qd, robot.max_qd)
- ub = np.array(robot.model.velocityLimit, dtype='double')
+ ub = np.array(robot.model.velocityLimit, dtype="double")
ub = np.clip(ub, -1 * robot.max_qd, robot.max_qd)
return partial(invKinmQP, lb=lb, ub=ub)
if args.clik_controller == "QPproxsuite":
@@ -289,28 +335,28 @@ def getClikController(args, robot):
b = np.ones(6) * 0.1
# proxsuite does lb <= Cx <= ub
C = np.eye(robot.model.nv)
- lb = -1 * np.array(robot.model.velocityLimit, dtype='double')
+ lb = -1 * np.array(robot.model.velocityLimit, dtype="double")
# we do additional clipping
lb = np.clip(lb, -1 * robot.max_qd, robot.max_qd)
- ub = np.array(robot.model.velocityLimit, dtype='double')
+ ub = np.array(robot.model.velocityLimit, dtype="double")
ub = np.clip(ub, -1 * robot.max_qd, robot.max_qd)
qp = proxsuite.proxqp.dense.QP(robot.model.nv, 6, robot.model.nv)
qp.init(H, g, A, b, G, lb, ub)
qp.solve()
return partial(QPproxsuite, qp, lb, ub)
- #if controller_name == "invKinmQPSingAvoidE_kI":
+ # if controller_name == "invKinmQPSingAvoidE_kI":
# return invKinmQPSingAvoidE_kI
- #if controller_name == "invKinmQPSingAvoidE_kM":
+ # if controller_name == "invKinmQPSingAvoidE_kM":
# return invKinmQPSingAvoidE_kM
- #if controller_name == "invKinmQPSingAvoidManipMax":
+ # if controller_name == "invKinmQPSingAvoidManipMax":
# return invKinmQPSingAvoidManipMax
# default
return partial(dampedPseudoinverse, args.tikhonov_damp)
-def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
+def controlLoopClik(robot: RobotManager, clik_controller, i, past_data):
"""
controlLoopClik
---------------
@@ -325,31 +371,31 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
T_w_e = robot.getT_w_e()
# first check whether we're at the goal
SEerror = T_w_e.actInv(robot.Mgoal)
- err_vector = pin.log6(SEerror).vector
+ err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
breakFlag = True
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# compute the joint velocities based on controller you passed
- #qd = clik_controller(J, err_vector, past_qd=past_data['dqs_cmd'][-1])
+ # qd = clik_controller(J, err_vector, past_qd=past_data['dqs_cmd'][-1])
qd = clik_controller(J, err_vector)
if qd is None:
- print(i)
+ print("the controller you chose didn't work, using dampedPseudoinverse instead")
qd = dampedPseudoinverse(1e-2, J, err_vector)
- else:
- print(i, "actually worked")
robot.sendQd(qd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
- log_item['err_norm'] = np.linalg.norm(err_vector).reshape((1,))
- # we're not saving here, but need to respect the API,
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
+ log_item["err_norm"] = np.linalg.norm(err_vector).reshape((1,))
+ # we're not saving here, but need to respect the API,
# hence the empty dict
- save_past_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+ save_past_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
return breakFlag, save_past_item, log_item
-def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform, i, past_data):
+def controlLoopClikDualArm(
+ robot: RobotManager, clik_controller, goal_transform, i, past_data
+):
"""
controlLoopClikDualArm
---------------
@@ -365,40 +411,43 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
log_item = {}
q = robot.getQ()
T_w_e_left, T_w_e_right = robot.getT_w_e()
- #
+ #
Mgoal_left = goal_transform.act(robot.Mgoal)
Mgoal_right = goal_transform.inverse().act(robot.Mgoal)
- #print("robot.Mgoal", robot.Mgoal)
- #print("Mgoal_left", Mgoal_left)
- #print("Mgoal_right", Mgoal_right)
+ # print("robot.Mgoal", robot.Mgoal)
+ # print("Mgoal_left", Mgoal_left)
+ # print("Mgoal_right", Mgoal_right)
SEerror_left = T_w_e_left.actInv(Mgoal_left)
SEerror_right = T_w_e_right.actInv(Mgoal_right)
- err_vector_left = pin.log6(SEerror_left).vector
- err_vector_right = pin.log6(SEerror_right).vector
+ err_vector_left = pin.log6(SEerror_left).vector
+ err_vector_right = pin.log6(SEerror_right).vector
- if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (np.linalg.norm(err_vector_right) < robot.args.goal_error):
+ if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (
+ np.linalg.norm(err_vector_right) < robot.args.goal_error
+ ):
breakFlag = True
J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
# compute the joint velocities based on controller you passed
# this works exactly the way you think it does:
# the velocities for the other arm are 0
- # what happens to the base hasn't been investigated but it seems ok
+ # what happens to the base hasn't been investigated but it seems ok
qd_left = clik_controller(J_left, err_vector_left)
qd_right = clik_controller(J_right, err_vector_right)
qd = qd_left + qd_right
robot.sendQd(qd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
+ # we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, {}, log_item
-#def clikDualArm(args, robot, goal, goal_transform, run=True):
+
+# def clikDualArm(args, robot, goal, goal_transform, run=True):
# """
# clikDualArm
# -----------
@@ -419,6 +468,7 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform
# else:
# return loop_manager
+
def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
breakFlag = False
log_item = {}
@@ -426,26 +476,29 @@ def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
T_w_e = robot.getT_w_e()
# first check whether we're at the goal
SEerror = T_w_e.actInv(robot.Mgoal)
- err_vector = pin.log6(SEerror).vector
+ err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
breakFlag = True
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# cut off base from jacobian
- J = J[:,3:]
+ J = J[:, 3:]
# compute the joint velocities based on controller you passed
qd = clik_controller(J, err_vector)
# add the missing velocities back
qd = np.hstack((np.zeros(3), qd))
robot.sendQd(qd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
+ # we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, {}, log_item
-def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controller, i, past_data):
+
+def moveUntilContactControlLoop(
+ args, robot: RobotManager, speed, clik_controller, i, past_data
+):
"""
moveUntilContactControlLoop
---------------
@@ -458,10 +511,10 @@ def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controll
log_item = {}
q = robot.getQ()
# break if wrench is nonzero basically
- #wrench = robot.getWrench()
+ # wrench = robot.getWrench()
# you're already giving the speed in the EE i.e. body frame
# so it only makes sense to have the wrench in the same frame
- #wrench = robot._getWrenchInEE()
+ # wrench = robot._getWrenchInEE()
wrench = robot.getWrench()
# and furthermore it's a reasonable assumption that you'll hit the thing
# in the direction you're going in.
@@ -482,27 +535,31 @@ def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controll
# compute the joint velocities.
qd = clik_controller(J, speed)
robot.sendQd(qd)
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['wrench'] = wrench.reshape((6,))
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["wrench"] = wrench.reshape((6,))
return breakFlag, {}, log_item
+
def moveUntilContact(args, robot, speed):
"""
moveUntilContact
-----
does clik until it feels something with the f/t sensor
"""
- assert type(speed) == np.ndarray
+ assert type(speed) == np.ndarray
clik_controller = getClikController(args, robot)
- controlLoop = partial(moveUntilContactControlLoop, args, robot, speed, clik_controller)
+ controlLoop = partial(
+ moveUntilContactControlLoop, args, robot, speed, clik_controller
+ )
# we're not using any past data or logging, hence the empty arguments
- log_item = {'wrench' : np.zeros(6)}
- log_item['qs'] = np.zeros((robot.model.nq,))
+ log_item = {"wrench": np.zeros(6)}
+ log_item["qs"] = np.zeros((robot.model.nq,))
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
print("Collision detected!!")
-def moveL(args, robot : RobotManager, goal_point):
+
+def moveL(args, robot: RobotManager, goal_point):
"""
moveL
-----
@@ -510,24 +567,27 @@ def moveL(args, robot : RobotManager, goal_point):
send a SE3 object as goal point.
if you don't care about rotation, make it np.zeros((3,3))
"""
- #assert type(goal_point) == pin.pinocchio_pywrap.SE3
+ # assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv),
- 'dqs_cmd' : np.zeros(robot.model.nv),
- 'err_norm' : np.zeros(1),
- }
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "dqs_cmd": np.zeros(robot.model.nv),
+ "err_norm": np.zeros(1),
+ }
save_past_dict = {
- 'dqs_cmd' : np.zeros(robot.model.nv),
- }
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ "dqs_cmd": np.zeros(robot.model.nv),
+ }
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
loop_manager.run()
-def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=True):
+
+def moveLDualArm(args, robot: RobotManager, goal_point, goal_transform, run=True):
"""
moveL
-----
@@ -535,22 +595,25 @@ def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=Tru
send a SE3 object as goal point.
if you don't care about rotation, make it np.zeros((3,3))
"""
- #assert type(goal_point) == pin.pinocchio_pywrap.SE3
+ # assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args, robot)
- controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform)
+ controlLoop = partial(
+ controlLoopClikDualArm, robot, clik_controller, goal_transform
+ )
# we're not using any past data or logging, hence the empty arguments
log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv),
- 'dqs_cmd' : np.zeros(robot.model.nv),
- }
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "dqs_cmd": np.zeros(robot.model.nv),
+ }
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
if run:
loop_manager.run()
else:
return loop_manager
+
# TODO: implement
def moveLFollowingLine(args, robot, goal_point):
"""
@@ -566,7 +629,10 @@ def moveLFollowingLine(args, robot, goal_point):
"""
pass
-def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, path_planner : ProcessManager, i, past_data):
+
+def cartesianPathFollowingWithPlannerControlLoop(
+ args, robot: RobotManager, path_planner: ProcessManager, i, past_data
+):
"""
cartesianPathFollowingWithPlanner
-----------------------------
@@ -579,31 +645,31 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
q = robot.getQ()
T_w_e = robot.getT_w_e()
p = T_w_e.translation[:2]
- path_planner.sendFreshestCommand({'p' : p})
+ path_planner.sendFreshestCommand({"p": p})
- # NOTE: it's pointless to recalculate the path every time
+ # NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
data = path_planner.getData()
if data == None:
if args.debug_prints:
print("got no path so no doing anything")
robot.sendQd(np.zeros(robot.model.nv))
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
if data == "done":
breakFlag = True
path_pol, path2D_untimed = data
- path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
# should be precomputed somewhere but this is nowhere near the biggest problem
max_base_v = np.linalg.norm(robot.model.robot.model.velocityLimit[:2])
- # 1) make 6D path out of [[x0,y0],...]
+ # 1) make 6D path out of [[x0,y0],...]
# that represents the center of the cart
pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
- #print(path2D_untimed)
- #for pp in pathSE3:
+ # print(path2D_untimed)
+ # for pp in pathSE3:
# print(pp.translation)
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualize_manipulator:
@@ -612,31 +678,36 @@ def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, pat
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# NOTE: obviously not path following but i want to see things working here
SEerror = T_w_e.actInv(pathSE3[-1])
- err_vector = pin.log6(SEerror).vector
- lb = -1*robot.model.robot.model.velocityLimit
+ err_vector = pin.log6(SEerror).vector
+ lb = -1 * robot.model.robot.model.velocityLimit
lb[1] = -0.001
ub = robot.model.robot.model.velocityLimit
ub[1] = 0.001
- #vel_cmd = invKinmQP(J, err_vector, lb=lb, ub=ub)
- vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
+ # vel_cmd = invKinmQP(J, err_vector, lb=lb, ub=ub)
+ vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
robot.sendQd(vel_cmd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- #time.sleep(0.01)
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ # time.sleep(0.01)
return breakFlag, save_past_dict, log_item
-def cartesianPathFollowingWithPlanner(args, robot : RobotManager, path_planner : ProcessManager):
- controlLoop = partial(cartesianPathFollowingWithPlannerControlLoop, args, robot, path_planner)
- log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv)
- }
+
+def cartesianPathFollowingWithPlanner(
+ args, robot: RobotManager, path_planner: ProcessManager
+):
+ controlLoop = partial(
+ cartesianPathFollowingWithPlannerControlLoop, args, robot, path_planner
+ )
+ log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
save_past_dict = {}
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
loop_manager.run()
-def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
+
+def controlLoopCompliantClik(args, robot: RobotManager, i, past_data):
"""
controlLoopClik
---------------
@@ -654,35 +725,42 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
# we need to overcome noise if we want to converge
if np.linalg.norm(wrench) < args.minimum_detectable_force_norm:
wrench = np.zeros(6)
- save_past_dict['wrench'] = copy.deepcopy(wrench)
- wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
+ save_past_dict["wrench"] = copy.deepcopy(wrench)
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(
+ np.array(past_data["wrench"]), axis=0
+ )
Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
wrench = Z @ wrench
# first check whether we're at the goal
SEerror = T_w_e.actInv(robot.Mgoal)
- err_vector = pin.log6(SEerror).vector
+ err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
breakFlag = True
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
# compute the joint velocities.
# just plug and play different ones
- qd = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * args.tikhonov_damp) @ err_vector
+ qd = (
+ J.T
+ @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * args.tikhonov_damp)
+ @ err_vector
+ )
tau = J.T @ wrench
- #tau = tau[:6].reshape((6,1))
+ # tau = tau[:6].reshape((6,1))
qd += args.alpha * tau
robot.sendQd(qd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
# get actual velocity, not the commanded one.
# although that would be fun to compare i guess
# TODO: have both, but call the compute control signal like it should be
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- log_item['wrench'] = wrench.reshape((6,))
- log_item['tau'] = tau.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["wrench"] = wrench.reshape((6,))
+ log_item["tau"] = tau.reshape((robot.model.nv,))
+ # we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, save_past_dict, log_item
+
# add a threshold for the wrench
def compliantMoveL(args, robot, goal_point, run=True):
"""
@@ -693,29 +771,32 @@ def compliantMoveL(args, robot, goal_point, run=True):
send a SE3 object as goal point.
if you don't care about rotation, make it np.zeros((3,3))
"""
-# assert type(goal_point) == pin.pinocchio_pywrap.SE3
+ # assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopCompliantClik, args, robot)
# we're not using any past data or logging, hence the empty arguments
log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'wrench' : np.zeros(6),
- 'tau' : np.zeros(robot.model.nv),
- 'dqs' : np.zeros(robot.model.nv),
- }
+ "qs": np.zeros(robot.model.nq),
+ "wrench": np.zeros(6),
+ "tau": np.zeros(robot.model.nv),
+ "dqs": np.zeros(robot.model.nv),
+ }
save_past_dict = {
- 'wrench': np.zeros(6),
- }
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ "wrench": np.zeros(6),
+ }
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
if run:
loop_manager.run()
else:
return loop_manager
-def clikCartesianPathIntoJointPath(args, robot, path, \
- clikController, q_init, plane_pose):
+def clikCartesianPathIntoJointPath(
+ args, robot, path, clikController, q_init, plane_pose
+):
"""
clikCartesianPathIntoJointPath
------------------------------
@@ -735,7 +816,7 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
----------
- path: cartesian path given in task frame
"""
- # we don't know how many there will be, so a linked list is
+ # we don't know how many there will be, so a linked list is
# clearly the best data structure here (instert is o(1) still,
# and we aren't pressed on time when turning it into an array later)
qs = []
@@ -748,22 +829,20 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
sim_args.fast_simulation = True
sim_args.real_time_plotting = False
sim_args.visualize_manipulator = False
- sim_args.save_log = False # we're not using sim robot outside of this
- sim_args.max_iterations = 10000 # more than enough
+ sim_args.save_log = False # we're not using sim robot outside of this
+ sim_args.max_iterations = 10000 # more than enough
sim_robot = RobotManager(sim_args)
sim_robot.q = q_init.copy()
sim_robot._step()
for pose in path:
moveL(sim_args, sim_robot, pose)
# loop logs is a dict, dict keys list preserves input order
- new_q = sim_robot.q.copy()
+ new_q = sim_robot.q.copy()
if args.viz_test_path:
# look into visualize.py for details on what's available
T_w_e = sim_robot.getT_w_e()
- robot.updateViz({"q" : new_q,
- "T_w_e" : T_w_e,
- "point" : T_w_e.copy()})
- #time.sleep(0.005)
+ robot.updateViz({"q": new_q, "T_w_e": T_w_e, "point": T_w_e.copy()})
+ # time.sleep(0.005)
qs.append(new_q[:6])
# plot this on the real robot
@@ -774,7 +853,7 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
# we're putting a dmp over this so we already have the timing ready
# TODO: make this general, you don't want to depend on other random
# arguments (make this one traj_time, then put tau0 = traj_time there
- t = np.linspace(0, args.tau0, len(qs)).reshape((len(qs),1))
+ t = np.linspace(0, args.tau0, len(qs)).reshape((len(qs), 1))
joint_trajectory = np.hstack((t, qs))
# TODO handle saving more consistently/intentionally
# (although this definitely works right now and isn't bad, just mid)
@@ -782,10 +861,13 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
# TODO: check if we actually need this and if not remove the saving
# whatever code uses this is responsible to log it if it wants it,
# let's not have random files around.
- np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=',', fmt='%.5f')
+ np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=",", fmt="%.5f")
return joint_trajectory
-def controlLoopClikDualArmsOnly(robot : RobotManager, clik_controller, goal_transform, i, past_data):
+
+def controlLoopClikDualArmsOnly(
+ robot: RobotManager, clik_controller, goal_transform, i, past_data
+):
"""
controlLoopClikDualArmsOnly
---------------
@@ -794,43 +876,44 @@ def controlLoopClikDualArmsOnly(robot : RobotManager, clik_controller, goal_tran
log_item = {}
q = robot.getQ()
T_w_e_left, T_w_e_right = robot.getT_w_e()
- #
+ #
Mgoal_left = robot.Mgoal.act(goal_transform)
Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
SEerror_left = T_w_e_left.actInv(Mgoal_left)
SEerror_right = T_w_e_right.actInv(Mgoal_right)
- err_vector_left = pin.log6(SEerror_left).vector
- err_vector_right = pin.log6(SEerror_right).vector
+ err_vector_left = pin.log6(SEerror_left).vector
+ err_vector_right = pin.log6(SEerror_right).vector
- if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (np.linalg.norm(err_vector_right) < robot.args.goal_error):
+ if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (
+ np.linalg.norm(err_vector_right) < robot.args.goal_error
+ ):
breakFlag = True
J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
- J_left = J_left[:,3:]
+ J_left = J_left[:, 3:]
J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
- J_right = J_right[:,3:]
-
+ J_right = J_right[:, 3:]
# compute the joint velocities based on controller you passed
qd_left = clik_controller(J_left, err_vector_left)
qd_right = clik_controller(J_right, err_vector_right)
- #lb, ub = (-0.05 * robot.model.robot.model.velocityLimit, 0.05 * robot.model.robot.model.velocityLimit)
- #qd_left = invKinmQP(J_left, err_vector_left, lb=lb[3:], ub=ub[3:])
- #qd_right = invKinmQP(J_right, err_vector_right, lb=lb[3:], ub=ub[3:])
+ # lb, ub = (-0.05 * robot.model.robot.model.velocityLimit, 0.05 * robot.model.robot.model.velocityLimit)
+ # qd_left = invKinmQP(J_left, err_vector_left, lb=lb[3:], ub=ub[3:])
+ # qd_right = invKinmQP(J_right, err_vector_right, lb=lb[3:], ub=ub[3:])
qd = qd_left + qd_right
qd = np.hstack((np.zeros(3), qd))
robot.sendQd(qd)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
- # we're not saving here, but need to respect the API,
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["dqs_cmd"] = qd.reshape((robot.model.nv,))
+ # we're not saving here, but need to respect the API,
# hence the empty dict
return breakFlag, {}, log_item
-def moveLDualArmsOnly(args, robot : RobotManager, goal_point, goal_transform, run=True):
+def moveLDualArmsOnly(args, robot: RobotManager, goal_point, goal_transform, run=True):
"""
moveL
-----
@@ -838,23 +921,26 @@ def moveLDualArmsOnly(args, robot : RobotManager, goal_point, goal_transform, ru
send a SE3 object as goal point.
if you don't care about rotation, make it np.zeros((3,3))
"""
- #assert type(goal_point) == pin.pinocchio_pywrap.SE3
+ # assert type(goal_point) == pin.pinocchio_pywrap.SE3
robot.Mgoal = copy.deepcopy(goal_point)
clik_controller = getClikController(args, robot)
- controlLoop = partial(controlLoopClikDualArmsOnly, robot, clik_controller, goal_transform)
+ controlLoop = partial(
+ controlLoopClikDualArmsOnly, robot, clik_controller, goal_transform
+ )
# we're not using any past data or logging, hence the empty arguments
log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv),
- 'dqs_cmd' : np.zeros(robot.model.nv),
- }
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nv),
+ "dqs_cmd": np.zeros(robot.model.nv),
+ }
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
if run:
loop_manager.run()
else:
return loop_manager
-if __name__ == "__main__":
+
+if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
Mgoal = robot.defineGoalPointCLI()
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index e9ab1fb6e7490e1f21bbfe31aff24305ee7cf3f5..ab9d336d4227e03147191d29e843d00b3ab8a4b1 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -26,6 +26,7 @@ from types import NoneType
from os import getpid
from functools import partial
import pickle
+import typing
# import ros stuff if you're rosing
# TODO: add more as needed
@@ -162,6 +163,8 @@ def getMinimalArgParser():
#if (args.gripper != "none") and args.simulation:
# raise NotImplementedError('Did not figure out how to put the gripper in \
# the simulation yet, sorry :/ . You can have only 1 these flags right now')
+ parser.add_argument('--visualize-collision-approximation', action=argparse.BooleanOptionalAction, \
+ help="whether you want to visualize the collision approximation used in controllers with obstacle avoidance", default=True)
return parser
@@ -297,6 +300,10 @@ class ControlLoopManager:
# T_base = self.robot_manager.data.oMi[1]
# self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
+ if self.args.visualize_collision_approximation:
+ pass
+ # TODO: here call robot manager's update ellipses function
+
if self.args.real_time_plotting:
# don't put new stuff in if it didn't handle the previous stuff.
# it's a plotter, who cares if it's late.
@@ -524,6 +531,10 @@ class RobotManager:
if args.visualize_manipulator:
side_function = partial(manipulatorVisualizer, self.model, self.collision_model, self.visual_model)
self.visualizer_manager = ProcessManager(args, side_function, {"q" : self.q.copy()}, 0)
+ if args.visualize_collision_approximation:
+ pass
+ # TODO: import the ellipses here, and write an update ellipse function
+ # then also call that in controlloopmanager
# initialize and connect the interfaces
if not args.pinocchio_only:
@@ -1193,7 +1204,7 @@ class ProcessManager:
# the key should be a string containing the command,
# and the value should be the data associated with the command,
# just to have some consistency
- def sendCommand(self, command : dict):
+ def sendCommand(self, command : typing.Union[dict, np.ndarray]):
"""
sendCommand
------------
@@ -1202,48 +1213,10 @@ class ProcessManager:
the maximum number of things in the command queue is arbitrarily
set to 5. if you need to get the result immediately,
calculate whatever you need in main, not in a side process.
- """
- if self.command_queue.qsize() < 5:
- self.command_queue.put_nowait(command)
-
- def sendFreshestCommand(self, command : dict):
- """
- sendFreshestCommand
- ------------
- assumes you're calling from controlLoop and that
- you want a non-blocking call.
- there can be only 1 command in the queue,
- and that it is the latest command you want to issue.
- since ControlLoopManager is the master of all processes,
- and it runs the fastest, most of the time the slave process
- won't pop the command currently in queue, so ControlLoopManager
- will have to update it,
- and this is to be done by emptying the queue and then putting in the latest command.
- shouldn't this be solved with shared memory?
- yes, absolutely. but then i need to write more code than i am
- willing to do right now, and it would complicate the code
- a bit more than it's worth, losing it's readability for
- non-computer science users.
- if the point was to write the fastest code, this wouldn't be in python anyway.
- it would be good to add an option to select whether
- a command is shared via a queue, or shared_memory, but
- this will be done only if the preformance is too low
- (it probably won't be because a bit of copy-ing will certainly
- be insignificant compared to any interesting matrix operation".
- NOTE: this assumes only the master is putting stuff in,
- and only the slave is popping it if there's something
- (the slave has to a blocking call for this to make sense).
- worst case the slave gets one cycle old data, but again,
- we're not optimizing all the way
- """
- if self.command_queue.qsize() < 1:
- #self.command_queue.get_nowait()
- self.command_queue.put_nowait(command)
- # TODO merge with send command under if which depends on comm_direction
- def sendCommandViaSharedMemory(self, command):
- """
+ if comm_direction == 3:
sendCommandViaSharedMemory
+ ---------------------------
instead of having a queue for the commands, have a shared memory variable.
this makes sense if you want to send the latest command only,
instead of stacking up old commands in a queue.
@@ -1252,11 +1225,16 @@ class ProcessManager:
do you need to pollute half of robotmanager or whatever else
to deal with this.
"""
- assert type(command) == np.ndarray
- assert command.shape == self.shared_command.shape
- self.shared_command_lock.acquire()
- self.shared_command[:] = command[:]
- self.shared_command_lock.release()
+ if self.command_queue.qsize() < 5:
+ self.command_queue.put_nowait(command)
+
+ if self.comm_direction == 3:
+ assert type(command) == np.ndarray
+ assert command.shape == self.shared_command.shape
+ self.shared_command_lock.acquire()
+ self.shared_command[:] = command[:]
+ self.shared_command_lock.release()
+
def getData(self):
if self.comm_direction != 3:
diff --git a/python/ur_simple_control/networking/__init__.py b/python/ur_simple_control/networking/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/networking/client.py b/python/ur_simple_control/networking/client.py
new file mode 100644
index 0000000000000000000000000000000000000000..6ad0507e2394507d8fa4d83e1b16b1eb2ad0c29c
--- /dev/null
+++ b/python/ur_simple_control/networking/client.py
@@ -0,0 +1,67 @@
+import socket
+import time
+import message_specs_pb2
+
+
+# TODO: finish
+def client(args, init_command, shm_name, shared_data_lock, shm_data):
+ """
+ server
+ -------
+ connects to a server, then receives messages from it.
+ offers latest message via shared memory
+
+ ex. host = "127.0.0.1"
+ ex. host_port = 7777
+ """
+ shm = shared_memory.SharedMemory(name=shm_name)
+ p_shared = np.ndarray((2,), dtype=np.float64, buffer=shm.buf)
+ lock.acquire()
+ p[:] = p_shared[:]
+ lock.release()
+
+
+host = "127.0.0.1"
+host_port = 7777
+host_addr = (host, host_port)
+
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.connect(host_addr)
+
+
+def parse_message(msg_raw):
+ parsed_messages = []
+ offset = 0
+ while True:
+ pb2_msg = message_specs_pb2.iteration_n_time()
+ pb2_msg.ParseFromString(msg_raw[offset:])
+ parsed_messages.append(pb2_msg)
+ offset += pb2_msg.ByteSize()
+ if offset >= len(msg_raw):
+ break
+ return parsed_messages
+
+
+for i in range(100000):
+ # TODO: receive only in multiples of n_bytes of your message
+ msg_raw = s.recv(1024)
+ print("=" * 20)
+ print("message length", len(msg_raw))
+ if len(msg_raw) == 0:
+ print("iteration:", i, "got nothing, going for next recv")
+ continue
+ # pb2_msg = message_specs_pb2.iteration_n_time()
+ # pb2_msg.ParseFromString(msg_raw)
+ parsed_messages = parse_message(msg_raw)
+ for j in range(len(parsed_messages)):
+ print("-------------------")
+ my_time = time.perf_counter()
+ diff = my_time - parsed_messages[j].time_sent
+ print("client iteration:", i)
+ print("server iteration:", parsed_messages[j].iteration_number)
+ print("client time:", my_time)
+ print("server time:", parsed_messages[j].time_sent)
+ print("time between sending and receiveing:", diff)
+
+s.close()
+print("client out")
diff --git a/python/ur_simple_control/networking/message_specs.proto b/python/ur_simple_control/networking/message_specs.proto
new file mode 100644
index 0000000000000000000000000000000000000000..a45edd8f1f7c56cf45c69a93e7a52a40e952f51b
--- /dev/null
+++ b/python/ur_simple_control/networking/message_specs.proto
@@ -0,0 +1,9 @@
+edition = "2023";
+
+message joint_angles { repeated double q = 1; }
+
+// graziano's message
+message wrenches {
+ repeated double wrench_mine = 1;
+ repeated double wrench_estimate = 2;
+}
diff --git a/python/ur_simple_control/networking/message_specs_pb2.py b/python/ur_simple_control/networking/message_specs_pb2.py
new file mode 100644
index 0000000000000000000000000000000000000000..5ae3bf8f0ee97fb26b8752ba437017f38a1becc0
--- /dev/null
+++ b/python/ur_simple_control/networking/message_specs_pb2.py
@@ -0,0 +1,38 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler. DO NOT EDIT!
+# NO CHECKED-IN PROTOBUF GENCODE
+# source: message_specs.proto
+# Protobuf Python Version: 5.29.2
+"""Generated protocol buffer code."""
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import runtime_version as _runtime_version
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf.internal import builder as _builder
+_runtime_version.ValidateProtobufRuntimeVersion(
+ _runtime_version.Domain.PUBLIC,
+ 5,
+ 29,
+ 2,
+ '',
+ 'message_specs.proto'
+)
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x13message_specs.proto\"\x19\n\x0cjoint_angles\x12\t\n\x01q\x18\x01 \x03(\x01\"8\n\x08wrenches\x12\x13\n\x0bwrench_mine\x18\x01 \x03(\x01\x12\x17\n\x0fwrench_estimate\x18\x02 \x03(\x01\x62\x08\x65\x64itionsp\xe8\x07')
+
+_globals = globals()
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'message_specs_pb2', _globals)
+if not _descriptor._USE_C_DESCRIPTORS:
+ DESCRIPTOR._loaded_options = None
+ _globals['_JOINT_ANGLES']._serialized_start=23
+ _globals['_JOINT_ANGLES']._serialized_end=48
+ _globals['_WRENCHES']._serialized_start=50
+ _globals['_WRENCHES']._serialized_end=106
+# @@protoc_insertion_point(module_scope)
diff --git a/python/ur_simple_control/networking/notes.md b/python/ur_simple_control/networking/notes.md
new file mode 100644
index 0000000000000000000000000000000000000000..4c826afe353a26d31effa6a88a75f87137291594
--- /dev/null
+++ b/python/ur_simple_control/networking/notes.md
@@ -0,0 +1,47 @@
+networking in smc
+-----------------
+as in the rest of the library, the simplest good option was chosen.
+in particular, serialization is done with protobuf,
+and the communication is done over directly over a socket.
+for every new use-case,
+you need to define new messages in protobuf, compile them,
+and import the compiled ..._pb2.py file.
+compilation is something like
+
+```bash
+protoc --proto_path=$(pwd) --python_out=$(pwd) $(pwd)/message_specs.proto
+```
+
+minimal overhead is added for everything to work.
+in particular, multiple the messages can parsed out of a single
+recv() because getting 2 messages before reading either can certainly happen.
+however, storing a buffer in case a message(s) is longer than 1024 bytes will not
+be implemented until a need for it arrives.
+this also means that if some many messages are piled up that one of them gets sliced up,
+the program breaks with a parsing error.
+since the current use case is sending up to 20 doubles at a time,
+buffer implementation is postponed.
+
+udp communication is also postponed for the time being
+because tcp works on time for the current use case.
+
+server
+-------
+the server is the sender in this case.
+since the client (receiver) is dependent on the sender,
+the sender needs to start first and bind to a port - hence
+the sender is the server.
+
+starts sending stuff once a client is connected.
+only supports one client literally just because there was no point
+to write more code for the current use-case.
+
+client
+------
+waits for messages. keeps latest value stored in a separate variable.
+you can ask for the data at any point, and you will be served with the
+latest data received (i.e. a copy of the stored variable).
+
+
+using the server or client
+------------------------
diff --git a/python/ur_simple_control/networking/server.py b/python/ur_simple_control/networking/server.py
new file mode 100644
index 0000000000000000000000000000000000000000..a7818fc807240636f5d937a2f8e9e7c78dd9fdbb
--- /dev/null
+++ b/python/ur_simple_control/networking/server.py
@@ -0,0 +1,32 @@
+import socket
+import time
+import message_specs_pb2
+
+
+# TODO: finish
+def server(args, init_command, shm_name, shared_data_lock, shm_data):
+ """
+ server
+ -------
+ listens for a connection, then sends messages to the singular accepted client
+
+ ex. host = "127.0.0.1"
+ ex. host_port = 7777
+ """
+ host_addr = (args.host, args.port)
+ s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ s.bind(host_addr)
+ s.listen()
+ print("server listening on", host_addr)
+ comm_socket, comm_addr = s.accept()
+ print("accepted a client", comm_addr)
+ for i in range(100000):
+ time.sleep(0.00001)
+ pb2_msg = message_specs_pb2.iteration_n_time()
+ pb2_msg.iteration_number = i
+ pb2_msg.time_sent = time.perf_counter()
+ # msg = pickle.dumps((i, time.time()))
+ comm_socket.send(pb2_msg.SerializeToString())
+ s.close()
+ comm_socket.close()
+ print("server exited")
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index fa5f37ab5db0c676be3261ea10cc89e083182edc..972fadcbd468a068f9b86d5a1ddffb8815dd29f6 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -1,17 +1,31 @@
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoEEPathFollowingOCP, createBaseAndEEPathFollowingOCP
+from ur_simple_control.managers import (
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+ ProcessManager,
+)
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import (
+ createCrocoIKOCP,
+ createCrocoEEPathFollowingOCP,
+ createBaseAndEEPathFollowingOCP,
+)
import pinocchio as pin
import crocoddyl
import numpy as np
from functools import partial
import types
import importlib.util
-if importlib.util.find_spec('mim_solvers'):
+
+if importlib.util.find_spec("mim_solvers"):
import mim_solvers
# TODO: put others here too
-if importlib.util.find_spec('shapely'):
- from ur_simple_control.path_generation.planner import path2D_timed, pathPointFromPathParam, path2D_to_SE3
+if importlib.util.find_spec("shapely"):
+ from ur_simple_control.path_generation.planner import (
+ path2D_timed,
+ pathPointFromPathParam,
+ path2D_to_SE3,
+ )
# solve ocp in side process
@@ -26,10 +40,11 @@ if importlib.util.find_spec('shapely'):
# and a small number of iterations, that might be feasible too,
# we need to see, there's no way to know.
# but doing it that way is certainly much easier to implement
-# and probably better.
+# and probably better.
# i'm pretty sure that's how croco devs & mim_people do mpc
-def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, goal, i, past_data):
+
+def CrocoIKMPCControlLoop(args, robot: RobotManager, solver, goal, i, past_data):
breakFlag = False
log_item = {}
save_past_dict = {}
@@ -38,12 +53,12 @@ def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, goal, i, past_data
if robot.robot_name == "yumi":
goal_left, goal_right = goal
SEerror = robot.getT_w_e().actInv(robot.Mgoal)
- err_vector = pin.log6(SEerror).vector
+ err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
-# print("Convergence achieved, reached destionation!")
+ # print("Convergence achieved, reached destionation!")
breakFlag = True
- # set initial state from sensor
+ # set initial state from sensor
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
# warmstart solver with previous solution
@@ -54,16 +69,17 @@ def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, goal, i, past_data
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
us = np.array(solver.us)
- vel_cmds = xs[1, robot.model.nq:]
+ vel_cmds = xs[1, robot.model.nq :]
robot.sendQd(vel_cmds)
- log_item['qs'] = x0[:robot.model.nq]
- log_item['dqs'] = x0[robot.model.nv:]
- log_item['dqs_cmd'] = vel_cmds
- log_item['u_tau'] = us[0]
-
+ log_item["qs"] = x0[: robot.model.nq]
+ log_item["dqs"] = x0[robot.model.nv :]
+ log_item["dqs_cmd"] = vel_cmds
+ log_item["u_tau"] = us[0]
+
return breakFlag, save_past_dict, log_item
+
def CrocoIKMPC(args, robot, goal, run=True):
"""
IKMPC
@@ -89,19 +105,29 @@ def CrocoIKMPC(args, robot, goal, run=True):
controlLoop = partial(CrocoIKMPCControlLoop, args, robot, solver)
log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nq),
- 'dqs_cmd' : np.zeros(robot.model.nv), # we're assuming full actuation here
- 'u_tau' : np.zeros(robot.model.nv),
- }
+ "qs": np.zeros(robot.model.nq),
+ "dqs": np.zeros(robot.model.nq),
+ "dqs_cmd": np.zeros(robot.model.nv), # we're assuming full actuation here
+ "u_tau": np.zeros(robot.model.nv),
+ }
save_past_dict = {}
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
if run:
loop_manager.run()
else:
return loop_manager
-def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
+
+def CrocoEndEffectorPathFollowingMPCControlLoop(
+ args,
+ robot: RobotManager,
+ solver: crocoddyl.SolverBoxFDDP,
+ path_planner: ProcessManager,
+ i,
+ past_data,
+):
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
@@ -119,28 +145,27 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
T_w_e = robot.getT_w_e()
p = T_w_e.translation[:2]
-
- # NOTE: it's pointless to recalculate the path every time
+ # NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
if type(path_planner) == types.FunctionType:
pathSE3 = path_planner(T_w_e, i)
else:
- path_planner.sendCommandViaSharedMemory(p)
+ path_planner.sendCommand(p)
data = path_planner.getData()
if data == None:
if args.debug_prints:
print("CTRL: got no path so not i won't move")
robot.sendQd(np.zeros(robot.model.nv))
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
if data == "done":
breakFlag = True
path_pol, path2D_untimed = data
- path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ path2D_untimed = np.array(path2D_untimed).reshape((-1, 2))
# who cares if the velocity is right,
# it should be kinda right so that we need less ocp iterations
# and that's it
@@ -154,7 +179,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
if args.visualize_manipulator:
if i % 20 == 0:
robot.visualizer_manager.sendCommand({"frame_path": pathSE3})
-
+
x0 = np.concatenate([robot.getQ(), robot.getQd()])
solver.problem.x0 = x0
# warmstart solver with previous solution
@@ -163,24 +188,29 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
us_init = list(solver.us[1:]) + [solver.us[-1]]
for i, runningModel in enumerate(solver.problem.runningModels):
- runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3[i]
- #runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = pathSE3[i]
+ runningModel.differential.costs.costs[
+ "gripperPose" + str(i)
+ ].cost.residual.reference = pathSE3[i]
+ # runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = pathSE3[i]
# idk if that's necessary
- solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3[-1]
- #solver.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = pathSE3[-1]
+ solver.problem.terminalModel.differential.costs.costs[
+ "gripperPose" + str(args.n_knots)
+ ].cost.residual.reference = pathSE3[-1]
+ # solver.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = pathSE3[-1]
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
us = np.array(solver.us)
- vel_cmds = xs[1, robot.model.nq:]
+ vel_cmds = xs[1, robot.model.nq :]
robot.sendQd(vel_cmds)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
+
def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
"""
CrocoEndEffectorPathFollowingMPC
@@ -205,18 +235,27 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
solver.solve(xs_init, us_init, args.max_solver_iter)
- controlLoop = partial(CrocoEndEffectorPathFollowingMPCControlLoop, args, robot, solver, path_planner)
- log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv)
- }
+ controlLoop = partial(
+ CrocoEndEffectorPathFollowingMPCControlLoop, args, robot, solver, path_planner
+ )
+ log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
save_past_dict = {}
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
loop_manager.run()
-def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager,
- grasp_pose, goal_transform, iter_n, past_data):
+def BaseAndEEPathFollowingMPCControlLoop(
+ args,
+ robot: RobotManager,
+ solver: crocoddyl.SolverBoxFDDP,
+ path_planner: ProcessManager,
+ grasp_pose,
+ goal_transform,
+ iter_n,
+ past_data,
+):
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
@@ -234,15 +273,15 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
T_w_e = robot.getT_w_e()
p = q[:2]
# NOTE: this is the actual position, not what the path suggested
- # whether this or path reference should be put in is debateable.
+ # whether this or path reference should be put in is debateable.
# this feels more correct to me.
- save_past_dict['path2D_untimed'] = p
- path_planner.sendCommandViaSharedMemory(p)
+ save_past_dict["path2D_untimed"] = p
+ path_planner.sendCommand(p)
###########################
# get path from planner #
###########################
- # NOTE: it's pointless to recalculate the path every time
+ # NOTE: it's pointless to recalculate the path every time
# if it's the same 2D path
# get the path from the base from the current base position onward.
# but we're still fast so who cares
@@ -251,29 +290,29 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
if args.debug_prints:
print("CTRL: got no path so not i won't move")
robot.sendQd(np.zeros(robot.model.nv))
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
if data == "done":
breakFlag = True
robot.sendQd(np.zeros(robot.model.nv))
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
##########################################
# construct timed 2D path for the base #
##########################################
path_pol_base, path2D_untimed_base = data
- path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
- # ideally should be precomputed somewhere
+ path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1, 2))
+ # ideally should be precomputed somewhere
max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
# base just needs timing on the path
path_base = path2D_timed(args, path2D_untimed_base, max_base_v)
# and it's of height 0 (i.e. the height of base's planar joint)
- path_base = np.hstack((path_base, np.zeros((len(path_base),1))))
-
+ path_base = np.hstack((path_base, np.zeros((len(path_base), 1))))
+
###################################################
# construct timed SE3 path for the end-effector #
###################################################
@@ -286,15 +325,17 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
# 4) turn the timed 2D path into an SE3 trajectory
# NOTE: this can be O(1) instead of O(n) but i can't be bothered
- path_arclength = np.linalg.norm(p - past_data['path2D_untimed'])
+ path_arclength = np.linalg.norm(p - past_data["path2D_untimed"])
handlebar_path_index = -1
- for i in range(-2, -1 * len(past_data['path2D_untimed']), -1):
+ for i in range(-2, -1 * len(past_data["path2D_untimed"]), -1):
if path_arclength > args.base_to_handlebar_preferred_distance:
handlebar_path_index = i
break
- path_arclength += np.linalg.norm(past_data['path2D_untimed'][i - 1] - past_data['path2D_untimed'][i])
+ path_arclength += np.linalg.norm(
+ past_data["path2D_untimed"][i - 1] - past_data["path2D_untimed"][i]
+ )
# i shouldn't need to copy-paste everything but what can you do
- path2D_handlebar_1_untimed = np.array(past_data['path2D_untimed'])
+ path2D_handlebar_1_untimed = np.array(past_data["path2D_untimed"])
# NOTE: BIG ASSUMPTION
# let's say we're computing on time, and that's the time spacing
# of previous path points.
@@ -307,11 +348,16 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
# TODO: actually save timing, pass t instead of i to controlLoops
# from controlLoopManager
# NOTE: this might not working when rosified so check that first
- time_past = np.linspace(0.0, args.past_window_size * robot.dt, args.past_window_size)
+ time_past = np.linspace(
+ 0.0, args.past_window_size * robot.dt, args.past_window_size
+ )
s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.n_knots)
- path2D_handlebar_1 = np.hstack((
- np.interp(s, time_past, path2D_handlebar_1_untimed[:,0]).reshape((-1,1)),
- np.interp(s, time_past, path2D_handlebar_1_untimed[:,1]).reshape((-1,1))))
+ path2D_handlebar_1 = np.hstack(
+ (
+ np.interp(s, time_past, path2D_handlebar_1_untimed[:, 0]).reshape((-1, 1)),
+ np.interp(s, time_past, path2D_handlebar_1_untimed[:, 1]).reshape((-1, 1)),
+ )
+ )
pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
pathSE3_handlebar_left = []
@@ -333,34 +379,51 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
us_init = list(solver.us[1:]) + [solver.us[-1]]
for i, runningModel in enumerate(solver.problem.runningModels):
- #print('adding base', path_base[i])
- #print("this was the prev ref", runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference)
- runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
+ # print('adding base', path_base[i])
+ # print("this was the prev ref", runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference)
+ runningModel.differential.costs.costs[
+ "base_translation" + str(i)
+ ].cost.residual.reference = path_base[i]
if robot.robot_name != "yumi":
- runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+ runningModel.differential.costs.costs[
+ "ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3_handlebar[i]
else:
- runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar_left[i]
- runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar_right[i]
+ runningModel.differential.costs.costs[
+ "l_ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3_handlebar_left[i]
+ runningModel.differential.costs.costs[
+ "r_ee_pose" + str(i)
+ ].cost.residual.reference = pathSE3_handlebar_right[i]
# idk if that's necessary
- solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
+ solver.problem.terminalModel.differential.costs.costs[
+ "base_translation" + str(args.n_knots)
+ ].cost.residual.reference = path_base[-1]
if robot.robot_name != "yumi":
- solver.problem.terminalModel.differential.costs.costs['ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+ solver.problem.terminalModel.differential.costs.costs[
+ "ee_pose" + str(args.n_knots)
+ ].cost.residual.reference = pathSE3_handlebar[-1]
else:
- solver.problem.terminalModel.differential.costs.costs['l_ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
- solver.problem.terminalModel.differential.costs.costs['r_ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+ solver.problem.terminalModel.differential.costs.costs[
+ "l_ee_pose" + str(args.n_knots)
+ ].cost.residual.reference = pathSE3_handlebar[-1]
+ solver.problem.terminalModel.differential.costs.costs[
+ "r_ee_pose" + str(args.n_knots)
+ ].cost.residual.reference = pathSE3_handlebar[-1]
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
us = np.array(solver.us)
- vel_cmds = xs[1, robot.model.nq:]
+ vel_cmds = xs[1, robot.model.nq :]
robot.sendQd(vel_cmds)
-
- log_item['qs'] = q.reshape((robot.model.nq,))
- log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
+
def BaseAndEEPathFollowingMPC(args, robot, path_planner):
"""
CrocoEndEffectorPathFollowingMPC
@@ -382,17 +445,17 @@ def BaseAndEEPathFollowingMPC(args, robot, path_planner):
us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
solver.solve(xs_init, us_init, args.max_solver_iter)
- controlLoop = partial(BaseAndEEPathFollowingMPCControlLoop, args, robot, solver, path_planner)
- log_item = {
- 'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nv)
- }
+ controlLoop = partial(
+ BaseAndEEPathFollowingMPCControlLoop, args, robot, solver, path_planner
+ )
+ log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
# TODO: put ensurance that save_past is not too small for this
# or make a specific argument for THIS past-moving-window size
# this is the end-effector's reference, so we should initialize that
# TODO: verify this initialization actually makes sense
T_w_e = robot.getT_w_e()
- save_past_dict = {'path2D_untimed' : T_w_e.translation[:2]}
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ save_past_dict = {"path2D_untimed": T_w_e.translation[:2]}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
loop_manager.run()
-
diff --git a/python/ur_simple_control/robots/robotmanager_abstract.py b/python/ur_simple_control/robots/robotmanager_abstract.py
new file mode 100644
index 0000000000000000000000000000000000000000..ff87510dffceb61f4e40835f0d0c622354787144
--- /dev/null
+++ b/python/ur_simple_control/robots/robotmanager_abstract.py
@@ -0,0 +1,769 @@
+import abc
+import argparse
+
+###################################
+# TODO: make everything different on each robot an abstract method.
+# TODO: try to retain non-robot defined functions like _step, getters, setters etc
+############################################
+
+
+class RobotManager(abc.ABC):
+ """
+ RobotManager:
+ ---------------
+ - design goal: rely on pinocchio as much as possible while
+ concealing obvious bookkeeping
+ - right now it is assumed you're running this on UR5e so some
+ magic numbers are just put to it.
+ this will be extended once there's a need for it.
+ - at this stage it's just a boilerplate reduction class
+ but the idea is to inherit it for more complicated things
+ with many steps, like dmp.
+ or just showe additional things in, this is python after all
+ - you write your controller separately,
+ and then drop it into this - there is a wrapper function you put
+ around the control loop which handles timing so that you
+ actually run at 500Hz and not more.
+ - this is probably not the most new-user friendly solution,
+ but it's designed for fastest idea to implementation rate.
+ - if this was a real programming language, all of these would really be private variables.
+ as it currently stands, "private" functions have the '_' prefix
+ while the public getters don't have a prefix.
+ - TODO: write out default arguments needed here as well
+ """
+
+ # just pass all of the arguments here and store them as is
+ # so as to minimize the amount of lines.
+ # might be changed later if that seems more appropriate
+ @abc.abstractmethod
+ def __init__(self, args):
+ self.args: type
+ self.pinocchio_only = args.pinocchio_only
+ if self.args.simulation:
+ self.args.robot_ip = "127.0.0.1"
+ # load model
+ # collision and visual models are none if args.visualize == False
+ self.robot_name = args.robot
+ if self.robot_name == "ur5e":
+ self.model, self.collision_model, self.visual_model, self.data = get_model()
+ if self.robot_name == "heron":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ heron_approximation()
+ )
+ if self.robot_name == "heronros":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ heron_approximation()
+ )
+ if self.robot_name == "mirros":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ mir_approximation()
+ )
+ # self.publisher_vel_base = create_publisher(msg.Twist, '/cmd_vel', 5)
+ # self.publisher_vel_base = publisher_vel_base
+ if self.robot_name == "gripperlessur5e":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ getGripperlessUR5e()
+ )
+ if self.robot_name == "yumi":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ get_yumi_model()
+ )
+
+ # create log manager if we're saving logs
+ if args.save_log:
+ self.log_manager = LogManager(args)
+
+ # ur specific magic numbers
+ # NOTE: all of this is ur-specific, and needs to be if-ed if other robots are added.
+ # TODO: this is 8 in pinocchio and that's what you actually use
+ # if we're being real lmao
+ # the TODO here is make this consistent obviously
+ self.n_arm_joints = 6
+ # last joint because pinocchio adds base frame as 0th joint.
+ # and since this is unintuitive, we add the other variable too
+ # so that the control designer doesn't need to think about such bs
+ # self.JOINT_ID = 6
+ self.ee_frame_id = self.model.getFrameId("tool0")
+ if self.robot_name == "yumi":
+ self.r_ee_frame_id = self.model.getFrameId("robr_joint_7")
+ self.l_ee_frame_id = self.model.getFrameId("robl_joint_7")
+ # JUST FOR TEST
+ # NOTE
+ # NOTE
+ # NOTE
+ self.ee_frame_id = self.model.getFrameId("robr_joint_7")
+ # self.ee_frame_id = self.model.getFrameId("hande_right_finger_joint")
+ # TODO: add -1 option here meaning as fast as possible
+ self.update_rate = args.ctrl_freq # Hz
+ self.dt = 1 / self.update_rate
+ # you better not give me crazy stuff
+ # and i'm not clipping it, you're fixing it
+ self.MAX_ACCELERATION = 1.7
+ assert args.acceleration <= self.MAX_ACCELERATION and args.acceleration > 0.0
+ # this is the number passed to speedj
+ self.acceleration = args.acceleration
+ # NOTE: this is evil and everything only works if it's set to 1
+ # you really should control the acceleration via the acceleration argument.
+ assert args.speed_slider <= 1.0 and args.acceleration > 0.0
+ # TODO: these are almost certainly higher
+ # NOTE and TODO: speed slider is evil, put it to 1, handle the rest yourself.
+ # NOTE: i have no idea what's the relationship between max_qdd and speed slider
+ # self.max_qdd = 1.7 * args.speed_slider
+ # NOTE: this is an additional kinda evil speed limitation (by this code, not UR).
+ # we're clipping joint velocities with this.
+ # if your controllers are not what you expect, you might be commanding a very high velocity,
+ # which is clipped, resulting in unexpected movement.
+ self.MAX_QD = 3.14
+ assert args.max_qd <= self.MAX_QD and args.max_qd > 0.0
+ self.max_qd = args.max_qd
+
+ self.gripper = None
+ if (self.args.gripper != "none") and not self.pinocchio_only:
+ if self.args.gripper == "robotiq":
+ self.gripper = RobotiqGripper()
+ self.gripper.connect(args.robot_ip, 63352)
+ self.gripper.activate()
+ if self.args.gripper == "onrobot":
+ self.gripper = TWOFG()
+
+ # TODO: specialize for each robot,
+ # add reasonable home positions
+ self.q = pin.randomConfiguration(
+ self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
+ )
+ if self.robot_name == "ur5e":
+ self.q[-1] = 0.0
+ self.q[-2] = 0.0
+ # v_q is the generalization of qd for every type of joint.
+ # for revolute joints it's qd, but for ex. the planar joint it's the body velocity.
+ self.v_q = np.zeros(self.model.nv)
+ # same note as v_q, but it's a_q.
+ self.a_q = np.zeros(self.model.nv)
+
+ # start visualize manipulator process if selected.
+ # has to be started here because it lives throughout the whole run
+ if args.visualize_manipulator:
+ side_function = partial(
+ manipulatorVisualizer,
+ self.model,
+ self.collision_model,
+ self.visual_model,
+ )
+ self.visualizer_manager = ProcessManager(
+ args, side_function, {"q": self.q.copy()}, 0
+ )
+ if args.visualize_collision_approximation:
+ pass
+ # TODO: import the ellipses here, and write an update ellipse function
+ # then also call that in controlloopmanager
+
+ # initialize and connect the interfaces
+ if not args.pinocchio_only:
+ # NOTE: you can't connect twice, so you can't have more than one RobotManager.
+ # 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(args.robot_ip)
+ self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
+ self.rtde_io = RTDEIOInterface(args.robot_ip)
+ self.rtde_io.setSpeedSlider(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()
+ else:
+ self.wrench_offset = np.zeros(6)
+
+ self.speed_slider = args.speed_slider
+
+ if args.pinocchio_only and args.start_from_current_pose:
+ self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
+ q = self.rtde_receive.getActualQ()
+ q.append(0.0)
+ q.append(0.0)
+ q = np.array(q)
+ self.q = q
+ if args.visualize_manipulator:
+ self.visualizer_manager.sendCommand({"q": q})
+
+ # do it once to get T_w_e
+ self._step()
+
+ #######################################################################
+ # getters which assume you called step() #
+ #######################################################################
+
+ def getQ(self):
+ return self.q.copy()
+
+ def getQd(self):
+ return self.v_q.copy()
+
+ def getT_w_e(self, q_given=None):
+ if self.robot_name != "yumi":
+ if q_given is None:
+ return self.T_w_e.copy()
+ else:
+ assert type(q_given) is np.ndarray
+ # calling these here is ok because we rely
+ # on robotmanager attributes instead of model.something
+ # (which is copying data, but fully separates state and computation,
+ # which is important in situations like this)
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ q_given,
+ np.zeros(self.model.nv),
+ np.zeros(self.model.nv),
+ )
+ # NOTE: this also returns the frame, so less copying possible
+ # pin.updateFramePlacements(self.model, self.data)
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ return self.data.oMf[self.ee_frame_id].copy()
+ else:
+ if q_given is None:
+ return self.T_w_e_left.copy(), self.T_w_e_right.copy().copy()
+ else:
+ assert type(q_given) is np.ndarray
+ # calling these here is ok because we rely
+ # on robotmanager attributes instead of model.something
+ # (which is copying data, but fully separates state and computation,
+ # which is important in situations like this)
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ q_given,
+ np.zeros(self.model.nv),
+ np.zeros(self.model.nv),
+ )
+ # NOTE: this also returns the frame, so less copying possible
+ # pin.updateFramePlacements(self.model, self.data)
+ pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+ return (
+ self.data.oMf[self.l_ee_frame_id].copy(),
+ self.data.oMf[self.r_ee_frame_id].copy(),
+ )
+
+ # this is in EE frame by default (handled in step which
+ # is assumed to be called before this)
+ def getWrench(self):
+ return self.wrench.copy()
+
+ def calibrateFT(self):
+ """
+ calibrateFT
+ -----------
+ Read from the f/t sensor a bit, average the results
+ and return the result.
+ This can be used to offset the bias of the f/t sensor.
+ NOTE: this is not an ideal solution.
+ ALSO TODO: test whether the offset changes when
+ the manipulator is in different poses.
+ """
+ ft_readings = []
+ print("Will read from f/t sensors for a some number of seconds")
+ print("and give you the average.")
+ print("Use this as offset.")
+ # NOTE: zeroFtSensor() needs to be called frequently because it drifts
+ # by quite a bit in a matter of minutes.
+ # if you are running something on the robot for a long period of time, you need
+ # to reapply zeroFtSensor() to get reasonable results.
+ # because the robot needs to stop for the zeroing to make sense,
+ # this is the responsibility of the user!!!
+ self.rtde_control.zeroFtSensor()
+ for i in range(2000):
+ start = time.time()
+ ft = self.rtde_receive.getActualTCPForce()
+ ft_readings.append(ft)
+ end = time.time()
+ diff = end - start
+ if diff < self.dt:
+ time.sleep(self.dt - diff)
+
+ ft_readings = np.array(ft_readings)
+ self.wrench_offset = np.average(ft_readings, axis=0)
+ print(self.wrench_offset)
+ return self.wrench_offset.copy()
+
+ def _step(self):
+ """
+ _step
+ ----
+ - the idea is to update everything that should be updated
+ on a step-by-step basis
+ - the actual problem this is solving is that you're not calling
+ forwardKinematics, an expensive call, more than once per step.
+ - within the TODO is to make all (necessary) variable private
+ so that you can rest assured that everything is handled the way
+ it's supposed to be handled. then have getters for these
+ private variables which return deepcopies of whatever you need.
+ that way the computations done in the control loop
+ can't mess up other things. this is important if you want
+ to switch between controllers during operation and have a completely
+ painless transition between them.
+ TODO: make the getQ, getQd and the rest here do the actual communication,
+ and make these functions private.
+ then have the deepcopy getters public.
+ also TODO: make ifs for the simulation etc.
+ this is less ifs overall right.
+ """
+ self._getQ()
+ self._getQd()
+ # self._getWrench()
+ # computeAllTerms is certainly not necessary btw
+ # but if it runs on time, does it matter? it makes everything available...
+ # (includes forward kinematics, all jacobians, all dynamics terms, energies)
+ # pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
+ pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
+ if self.robot_name != "yumi":
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+ else:
+ pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+ self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
+ self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
+ # wrench in EE should obviously be the default
+ self._getWrenchInEE(step_called=True)
+ # this isn't real because we're on a velocity-controlled robot,
+ # so this is actually None (no tau, no a_q, as expected)
+ self.a_q = self.data.ddq
+ # TODO NOTE: you'll want to do the additional math for
+ # torque controlled robots here, but it's ok as is rn
+
+ def setSpeedSlider(self, value):
+ """
+ setSpeedSlider
+ ---------------
+ update in all places
+ """
+ assert value <= 1.0 and value > 0.0
+ if not self.args.pinocchio_only:
+ self.rtde_io.setSpeedSlider(value)
+ self.speed_slider = value
+
+ def _getQ(self):
+ """
+ _getQ
+ -----
+ NOTE: private function for use in _step(), use the getter getQ()
+ urdf treats gripper as two prismatic joints,
+ but they do not affect the overall movement
+ of the robot, so we add or remove 2 items to the joint list.
+ also, the gripper is controlled separately so we'd need to do this somehow anyway
+ NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
+ TODO: make work for new gripper
+ """
+ if not self.pinocchio_only:
+ q = self.rtde_receive.getActualQ()
+ if self.args.gripper == "robotiq":
+ # TODO: make it work or remove it
+ # self.gripper_past_pos = self.gripper_pos
+ # this is pointless by itself
+ self.gripper_pos = self.gripper.get_current_position()
+ # the /255 is to get it dimensionless.
+ # the gap is 5cm,
+ # thus half the gap is 0.025m (and we only do si units here).
+ q.append((self.gripper_pos / 255) * 0.025)
+ q.append((self.gripper_pos / 255) * 0.025)
+ else:
+ # just fill it with zeros otherwise
+ if self.robot_name == "ur5e":
+ q.append(0.0)
+ q.append(0.0)
+ # let's just have both options for getting q, it's just a 8d float list
+ # readability is a somewhat subjective quality after all
+ q = np.array(q)
+ self.q = q
+
+ # TODO remove evil hack
+ def _getT_w_e(self, q_given=None):
+ """
+ _getT_w_e
+ -----
+ NOTE: private function, use the getT_w_e() getter
+ urdf treats gripper as two prismatic joints,
+ but they do not affect the overall movement
+ of the robot, so we add or remove 2 items to the joint list.
+ also, the gripper is controlled separately so we'd need to do this somehow anyway
+ NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO.
+ NOTE: don't use this if use called _step() because it repeats forwardKinematics
+ """
+ test = True
+ try:
+ test = q_given.all() == None
+ print(test)
+ print(q_given)
+ except AttributeError:
+ test = True
+
+ if test:
+ if not self.pinocchio_only:
+ q = self.rtde_receive.getActualQ()
+ if self.args.gripper == "robotiq":
+ # TODO: make it work or remove it
+ # self.gripper_past_pos = self.gripper_pos
+ # this is pointless by itself
+ self.gripper_pos = self.gripper.get_current_position()
+ # the /255 is to get it dimensionless.
+ # the gap is 5cm,
+ # thus half the gap is 0.025m (and we only do si units here).
+ q.append((self.gripper_pos / 255) * 0.025)
+ q.append((self.gripper_pos / 255) * 0.025)
+ else:
+ # just fill it with zeros otherwise
+ q.append(0.0)
+ q.append(0.0)
+ else:
+ q = self.q
+ else:
+ q = copy.deepcopy(q_given)
+ q = np.array(q)
+ self.q = q
+ pin.forwardKinematics(self.model, self.data, q)
+ if self.robot_name != "yumi":
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
+ else:
+ pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
+ self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
+ self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
+ # NOTE: VERY EVIL, to bugfix things that depend on this like wrench (which we don't have on yumi)
+ # self.T_w_e = self.data.oMf[self.l_ee_frame_id].copy()
+
+ def _getQd(self):
+ """
+ _getQd
+ -----
+ NOTE: private function, use the _getQd() getter
+ same note as _getQ.
+ TODO NOTE: atm there's no way to get current gripper velocity.
+ this means you'll probably want to read current positions and then finite-difference
+ to get the velocity.
+ as it stands right now, we'll just pass zeros in because I don't need this ATM
+ """
+ if not self.pinocchio_only:
+ qd = self.rtde_receive.getActualQd()
+ if self.args.gripper:
+ # TODO: this doesn't work because we're not ensuring stuff is called
+ # at every timestep
+ # self.gripper_vel = (gripper.get_current_position() - self.gripper_pos) / self.dt
+ # so it's just left unused for now - better give nothing than wrong info
+ self.gripper_vel = 0.0
+ # the /255 is to get it dimensionless
+ # the gap is 5cm
+ # thus half the gap is 0.025m and we only do si units here
+ # no need to deepcopy because only literals are passed
+ qd.append(self.gripper_vel)
+ qd.append(self.gripper_vel)
+ else:
+ # just fill it with zeros otherwise
+ qd.append(0.0)
+ qd.append(0.0)
+ # let's just have both options for getting q, it's just a 8d float list
+ # readability is a somewhat subjective quality after all
+ qd = np.array(qd)
+ self.v_q = qd
+
+ def _getWrenchRaw(self):
+ """
+ _getWrench
+ -----
+ different things need to be send depending on whether you're running a simulation,
+ you're on a real robot, you're running some new simulator bla bla. this is handled
+ here because this things depend on the arguments which are manager here (hence the
+ class name RobotManager)
+ """
+ if not self.pinocchio_only:
+ wrench = np.array(self.rtde_receive.getActualTCPForce())
+ else:
+ raise NotImplementedError("Don't have time to implement this right now.")
+
+ def _getWrench(self):
+ if not self.pinocchio_only:
+ self.wrench = (
+ np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
+ )
+ else:
+ # TODO: do something better here (at least a better distribution)
+ self.wrench = np.random.random(self.n_arm_joints)
+
+ def _getWrenchInEE(self, step_called=False):
+ if self.robot_name != "yumi":
+ if not self.pinocchio_only:
+ self.wrench = (
+ np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
+ )
+ else:
+ # TODO: do something better here (at least a better distribution)
+ self.wrench = np.random.random(self.n_arm_joints)
+ if not step_called:
+ self._getT_w_e()
+ # NOTE: this mapping is equivalent to having a purely rotational action
+ # this is more transparent tho
+ 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
+ else:
+ self.wrench = np.zeros(6)
+
+ def sendQd(self, qd):
+ """
+ sendQd
+ -----
+ different things need to be send depending on whether you're running a simulation,
+ you're on a real robot, you're running some new simulator bla bla. this is handled
+ here because this things depend on the arguments which are manager here (hence the
+ class name RobotManager)
+ """
+ # we're hiding the extra 2 prismatic joint shenanigans from the control writer
+ # because there you shouldn't need to know this anyway
+ if self.robot_name == "ur5e":
+ qd_cmd = qd[:6]
+ # np.clip is ok with bounds being scalar, it does what it should
+ # (but you can also give it an array)
+ qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
+ if not self.pinocchio_only:
+ # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
+ self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+ else:
+ # this one takes all 8 elements of qd since we're still in pinocchio
+ # this is ugly, todo: fix
+ qd = qd[:6]
+ qd = qd_cmd.reshape((6,))
+ qd = list(qd)
+ qd.append(0.0)
+ qd.append(0.0)
+ qd = np.array(qd)
+ self.v_q = qd
+ self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
+ if self.robot_name == "heron":
+ # y-direction is not possible on heron
+ qd_cmd = np.clip(
+ qd, -1 * self.model.velocityLimit, self.model.velocityLimit
+ )
+ # qd[1] = 0
+ self.v_q = qd_cmd
+ self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+
+ if self.robot_name == "heronros":
+ # y-direction is not possible on heron
+ qd[1] = 0
+ cmd_msg = msg.Twist()
+ cmd_msg.linear.x = qd[0]
+ cmd_msg.angular.z = qd[2]
+ # print("about to publish", cmd_msg)
+ self.publisher_vel_base.publish(cmd_msg)
+ # 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)
+
+ if self.robot_name == "mirros":
+ # y-direction is not possible on heron
+ qd[1] = 0
+ cmd_msg = msg.Twist()
+ cmd_msg.linear.x = qd[0]
+ cmd_msg.angular.z = qd[2]
+ # print("about to publish", cmd_msg)
+ self.publisher_vel_base.publish(cmd_msg)
+ # 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)
+
+ if self.robot_name == "gripperlessur5e":
+ qd_cmd = np.clip(qd, -1 * self.max_qd, self.max_qd)
+ if not self.pinocchio_only:
+ self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+ else:
+ self.v_q = qd_cmd
+ self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+
+ if self.robot_name == "yumi":
+ qd_cmd = np.clip(
+ qd, -0.01 * self.model.velocityLimit, 0.01 * self.model.velocityLimit
+ )
+ self.v_q = qd_cmd
+ # if self.args.pinocchio_only:
+ # self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+ # else:
+ # qd_base = qd[:3]
+ # qd_left = qd[3:10]
+ # qd_right = qd[10:]
+ # self.publisher_vel_base(qd_base)
+ # self.publisher_vel_left(qd_left)
+ # self.publisher_vel_right(qd_right)
+ empty_msg = JointState()
+ for i in range(29):
+ empty_msg.velocity.append(0.0)
+ msg = empty_msg
+ msg.header.stamp = Time().to_msg()
+ for i in range(3):
+ msg.velocity[i] = qd_cmd[i]
+ for i in range(15, 29):
+ msg.velocity[i] = qd_cmd[i - 12]
+
+ self.publisher_joints_cmd.publish(msg)
+
+ def openGripper(self):
+ if self.gripper is None:
+ if self.args.debug_prints:
+ print(
+ "you didn't select a gripper (no gripper is the default parameter) so no gripping for you"
+ )
+ return
+ if (not self.args.simulation) and (not self.args.pinocchio_only):
+ self.gripper.open()
+ else:
+ print("not implemented yet, so nothing is going to happen!")
+
+ def closeGripper(self):
+ if self.gripper is None:
+ if self.args.debug_prints:
+ print(
+ "you didn't select a gripper (no gripper is the default parameter) so no gripping for you"
+ )
+ return
+ if (not self.args.simulation) and (not self.args.pinocchio_only):
+ self.gripper.close()
+ else:
+ print("not implemented yet, so nothing is going to happen!")
+
+ #######################################################################
+ # utility functions #
+ #######################################################################
+
+ def defineGoalPointCLI(self):
+ """
+ defineGoalPointCLI
+ ------------------
+ NOTE: this assume _step has not been called because it's run before the controlLoop
+ --> best way to handle the goal is to tell the user where the gripper is
+ in both UR tcp frame and with pinocchio and have them
+ manually input it when running.
+ this way you force the thinking before the moving,
+ but you also get to view and analyze the information first
+ TODO get the visual thing you did in ivc project with sliders also.
+ it's just text input for now because it's totally usable, just not superb.
+ but also you do want to have both options. obviously you go for the sliders
+ in the case you're visualizing, makes no sense otherwise.
+ """
+ self._getQ()
+ q = self.getQ()
+ # define goal
+ pin.forwardKinematics(self.model, self.data, np.array(q))
+ pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
+ T_w_e = self.data.oMf[self.ee_frame_id]
+ print("You can only specify the translation right now.")
+ if not self.pinocchio_only:
+ print(
+ "In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles"
+ )
+ print(
+ "Here's where the robot is currently. Ensure you know what the base frame is first."
+ )
+ print(
+ "base frame end-effector pose from pinocchio:\n",
+ *self.data.oMi[6].translation.round(4),
+ *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4)
+ )
+ print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
+ # remain with the current orientation
+ # TODO: add something, probably rpy for orientation because it's the least number
+ # of numbers you need to type in
+ Mgoal = T_w_e.copy()
+ # this is a reasonable way to do it too, maybe implement it later
+ # Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+ # do a while loop until this is parsed correctly
+ while True:
+ goal = input(
+ "Please enter the target end-effector position in the x.x,y.y,z.z format: "
+ )
+ try:
+ e = "ok"
+ goal_list = goal.split(",")
+ for i in range(len(goal_list)):
+ goal_list[i] = float(goal_list[i])
+ except:
+ e = exc_info()
+ print("The input is not in the expected format. Try again.")
+ print(e)
+ if e == "ok":
+ Mgoal.translation = np.array(goal_list)
+ break
+ print("this is goal pose you defined:\n", Mgoal)
+
+ # NOTE i'm not deepcopying this on purpose
+ # but that might be the preferred thing, we'll see
+ self.Mgoal = Mgoal
+ if self.args.visualize_manipulator:
+ # TODO document this somewhere
+ self.visualizer_manager.sendCommand({"Mgoal": Mgoal})
+ return Mgoal
+
+ def killManipulatorVisualizer(self):
+ """
+ killManipulatorVisualizer
+ ---------------------------
+ if you're using the manipulator visualizer, you want to start it only once.
+ because you start the meshcat server, initialize the manipulator and then
+ do any subsequent changes with that server. there's no point in restarting.
+ but this means you have to kill it manually, because the ControlLoopManager
+ can't nor should know whether this is the last control loop you're running -
+ RobotManager has to handle the meshcat server.
+ and in this case the user needs to say when the tasks are done.
+ """
+ self.visualizer_manager.terminateProcess()
+
+ def stopRobot(self):
+ if not self.args.pinocchio_only:
+ print("stopping via freedrive lel")
+ self.rtde_control.freedriveMode()
+ time.sleep(0.5)
+ self.rtde_control.endFreedriveMode()
+
+ def setFreedrive(self):
+ if self.robot_name in ["ur5e", "gripperlessur5e"]:
+ self.rtde_control.freedriveMode()
+ else:
+ raise NotImplementedError("freedrive function only written for ur5e")
+
+ def unSetFreedrive(self):
+ if self.robot_name in ["ur5e", "gripperlessur5e"]:
+ self.rtde_control.endFreedriveMode()
+ else:
+ raise NotImplementedError("freedrive function only written for ur5e")
+
+ def updateViz(self, viz_dict: dict):
+ """
+ updateViz
+ ---------
+ updates the viz and only the viz according to arguments
+ NOTE: this function does not change internal variables!
+ because it shouldn't - it should only update the visualizer
+ """
+ if self.args.visualize_manipulator:
+ self.visualizer_manager.sendCommand(viz_dict)
+ else:
+ if self.args.debug_prints:
+ print("you didn't select viz")
+
+ def set_publisher_vel_base(self, publisher_vel_base):
+ self.publisher_vel_base = publisher_vel_base
+ print("set vel_base_publisher into robotmanager")
+
+ def set_publisher_vel_left(self, publisher_vel_left):
+ self.publisher_vel_left = publisher_vel_left
+ print("set vel_left_publisher into robotmanager")
+
+ def set_publisher_vel_right(self, publisher_vel_right):
+ self.publisher_vel_right = publisher_vel_right
+ print("set vel_right_publisher into robotmanager")
+
+ def set_publisher_joints_cmd(self, publisher_joints_cmd):
+ self.publisher_joints_cmd = publisher_joints_cmd
+ print("set publisher_joints_cmd into RobotManager")
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-311.pyc
index 6156d32d834e84180a79651492a8f37efd9e6ff9..e724219da54124138de30d0dbdabe960793bbf54 100644
Binary files a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index aa9f8054b344ab0333701bfe20c412a83188c33e..38b4a58b759b04ba21ce8753bda283693c29772c 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc differ
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc
index e29f229bd9772a126ab3b1fdef93916ff8259324..c34b2cf67deaa3275c474abf33199ce0e6406ff1 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc differ