diff --git a/python/examples/__init__.py b/python/examples/__init__.py
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling/cart_pulling.py
similarity index 100%
rename from python/examples/cart_pulling.py
rename to python/examples/cart_pulling/cart_pulling.py
diff --git a/python/examples/cart_pulling_notes.md b/python/examples/cart_pulling/cart_pulling_notes.md
similarity index 100%
rename from python/examples/cart_pulling_notes.md
rename to python/examples/cart_pulling/cart_pulling_notes.md
diff --git a/python/examples/clik.py b/python/examples/cartesian_space/clik.py
similarity index 100%
rename from python/examples/clik.py
rename to python/examples/cartesian_space/clik.py
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
deleted file mode 100644
index e6921bbb54557324fad8f035b6481488b67b90e0..0000000000000000000000000000000000000000
--- a/python/examples/crocoddyl_mpc.py
+++ /dev/null
@@ -1,84 +0,0 @@
-import numpy as np
-from smc import getMinimalArgParser, getRobotFromArgs
-from smc.control.optimal_control.crocoddyl_optimal_control import (
- createCrocoIKOCP,
- solveCrocoOCP,
-)
-from smc.control.optimal_control.util import get_OCP_args
-from smc.control.optimal_control.crocoddyl_mpc import CrocoIKMPC
-from smc.control.cartesian_space import getClikArgs
-import pinocchio as pin
-import importlib.util
-import argcomplete
-
-
-def get_args():
- parser = getMinimalArgParser()
- parser = get_OCP_args(parser)
- parser = getClikArgs(parser) # literally just for goal error
- argcomplete.autocomplete(parser)
- args = parser.parse_args()
- return args
-
-
-if __name__ == "__main__":
- if importlib.util.find_spec("mim_solvers"):
- import mim_solvers
-
- args = get_args()
- robot = getRobotFromArgs(args)
- # TODO: put this back for nicer demos
- # Mgoal = robot.defineGoalPointCLI()
- Mgoal = pin.SE3.Random()
- if robot.robot_name == "yumi":
- Mgoal.rotation = np.eye(3)
- Mgoal_transform = pin.SE3.Identity()
- Mgoal_transform.translation[1] = 0.1
- Mgoal_left = Mgoal_transform.act(Mgoal)
- Mgoal_right = Mgoal_transform.inverse().act(Mgoal)
- Mgoal = (Mgoal_left, Mgoal_right)
-
- robot.Mgoal = Mgoal.copy()
- if args.visualizer:
- # TODO document this somewhere
- robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
- # create and solve the optimal control problem of
- # getting from current to goal end-effector position.
- # reference is position and velocity reference (as a dictionary),
- # while solver is a crocoddyl object containing a lot more information
- # starting state
- x0 = np.concatenate([robot.q, robot.v])
- problem = createCrocoIKOCP(args, robot, x0, Mgoal)
-
- # NOTE: this might be useful if we solve with a large time horizon,
- # lower frequency, and then follow the predicted trajectory with velocity p-control
- # this shouldn't really depend on x0 but i can't be bothered
- # reference, solver = solveCrocoOCP(args, robot, problem, x0)
- # if args.solver == "boxfddp":
- # log = solver.getCallbacks()[1]
- # crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
- # if args.solver == "csqp":
- # log = solver.getCallbacks()[1]
- # mim_solvers.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
-
- # we need a way to follow the reference trajectory,
- # both because there can be disturbances,
- # and because it is sampled at a much lower frequency
- # followKinematicJointTrajP(args, robot, reference)
-
- CrocoIKMPC(args, robot, Mgoal)
-
- print("final position:")
- print(robot.getT_w_e())
-
- if args.real:
- robot.stopRobot()
-
- if args.visualizer:
- robot.killManipulatorVisualizer()
-
- if args.save_log:
- robot._log_manager.saveLog()
- robot._log_manager.plotAllControlLoops()
-
- # loop_manager.stopHandler(None, None)
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing/drawing_from_input_drawing.py
similarity index 100%
rename from python/examples/drawing_from_input_drawing.py
rename to python/examples/drawing/drawing_from_input_drawing.py
diff --git a/python/examples/plane_pose.pickle_save b/python/examples/drawing/plane_pose.pickle_save
similarity index 100%
rename from python/examples/plane_pose.pickle_save
rename to python/examples/drawing/plane_pose.pickle_save
diff --git a/python/examples/wiping_path.csv_save b/python/examples/drawing/wiping_path.csv_save
similarity index 100%
rename from python/examples/wiping_path.csv_save
rename to python/examples/drawing/wiping_path.csv_save
diff --git a/python/examples/point_impedance_control.py b/python/examples/impedance/point_impedance_control.py
similarity index 100%
rename from python/examples/point_impedance_control.py
rename to python/examples/impedance/point_impedance_control.py
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
deleted file mode 100644
index 179460bd485474e866e136d7685823fdfb2f1e01..0000000000000000000000000000000000000000
--- a/python/examples/joint_trajectory.csv
+++ /dev/null
@@ -1,187 +0,0 @@
-0.00000,0.53253,-1.30334,-1.52085,-1.05683,2.47804,-0.62074
-0.08065,0.53252,-1.30337,-1.52080,-1.05684,2.47805,-0.62074
-0.16129,0.53453,-1.30974,-1.50563,-1.06203,2.47638,-0.61634
-0.24194,0.54595,-1.31977,-1.47230,-1.07385,2.46769,-0.60187
-0.32258,0.55266,-1.32414,-1.45596,-1.07980,2.46260,-0.59417
-0.40323,0.56163,-1.32845,-1.43722,-1.08672,2.45579,-0.58450
-0.48387,0.56504,-1.32991,-1.43048,-1.08923,2.45320,-0.58091
-0.56452,0.57036,-1.33149,-1.42126,-1.09265,2.44914,-0.57542
-0.64516,0.57777,-1.33292,-1.40990,-1.09687,2.44348,-0.56795
-0.72581,0.58637,-1.33341,-1.39890,-1.10092,2.43687,-0.55944
-0.80645,0.59562,-1.33387,-1.38741,-1.10518,2.42973,-0.55048
-0.88710,0.60467,-1.33393,-1.37703,-1.10904,2.42271,-0.54187
-0.96774,0.61330,-1.33305,-1.36893,-1.11201,2.41599,-0.53381
-1.04839,0.61886,-1.33212,-1.36441,-1.11364,2.41163,-0.52866
-1.12903,0.62256,-1.33100,-1.36231,-1.11435,2.40872,-0.52526
-1.20968,0.62513,-1.32995,-1.36136,-1.11463,2.40670,-0.52292
-1.29032,0.62947,-1.32740,-1.36109,-1.11455,2.40328,-0.51898
-1.37097,0.63595,-1.32285,-1.36208,-1.11383,2.39815,-0.51313
-1.45161,0.64183,-1.31805,-1.36422,-1.11266,2.39349,-0.50788
-1.53226,0.64623,-1.31412,-1.36648,-1.11151,2.38998,-0.50398
-1.61290,0.65053,-1.30980,-1.36955,-1.11001,2.38655,-0.50019
-1.69355,0.65600,-1.30380,-1.37439,-1.10773,2.38218,-0.49540
-1.77419,0.65985,-1.29884,-1.37912,-1.10555,2.37909,-0.49206
-1.85484,0.66525,-1.29033,-1.38849,-1.10135,2.37474,-0.48738
-1.93548,0.66732,-1.28692,-1.39235,-1.09962,2.37307,-0.48560
-2.01613,0.66955,-1.28287,-1.39718,-1.09748,2.37127,-0.48368
-2.09677,0.67181,-1.27808,-1.40327,-1.09480,2.36944,-0.48174
-2.17742,0.67563,-1.26385,-1.42413,-1.08582,2.36632,-0.47841
-2.25806,0.67728,-1.24987,-1.44660,-1.07627,2.36495,-0.47690
-2.33871,0.67695,-1.23938,-1.46488,-1.06860,2.36518,-0.47708
-2.41935,0.67529,-1.22718,-1.48725,-1.05933,2.36649,-0.47835
-2.50000,0.67136,-1.21267,-1.51552,-1.04781,2.36961,-0.48150
-2.58065,0.66755,-1.20228,-1.53659,-1.03934,2.37264,-0.48462
-2.66129,0.66488,-1.19618,-1.54932,-1.03428,2.37476,-0.48682
-2.74194,0.66309,-1.19252,-1.55713,-1.03119,2.37619,-0.48830
-2.82258,0.66108,-1.18910,-1.56473,-1.02821,2.37779,-0.48998
-2.90323,0.65850,-1.18505,-1.57391,-1.02464,2.37984,-0.49215
-2.98387,0.65621,-1.18188,-1.58134,-1.02177,2.38166,-0.49409
-3.06452,0.65273,-1.17767,-1.59160,-1.01784,2.38442,-0.49705
-3.14516,0.64717,-1.17222,-1.60590,-1.01245,2.38884,-0.50183
-3.22581,0.64224,-1.16836,-1.61697,-1.00835,2.39274,-0.50612
-3.30645,0.63673,-1.16503,-1.62777,-1.00441,2.39709,-0.51096
-3.38710,0.62993,-1.16190,-1.63951,-1.00022,2.40245,-0.51702
-3.46774,0.62148,-1.15930,-1.65207,-0.99587,2.40909,-0.52464
-3.54839,0.61533,-1.15783,-1.66057,-0.99299,2.41390,-0.53026
-3.62903,0.60983,-1.15685,-1.66768,-0.99064,2.41820,-0.53535
-3.70968,0.60149,-1.15640,-1.67688,-0.98771,2.42468,-0.54316
-3.79032,0.58864,-1.15810,-1.68750,-0.98462,2.43462,-0.55544
-3.87097,0.57937,-1.16053,-1.69343,-0.98311,2.44175,-0.56449
-3.95161,0.56884,-1.16450,-1.69843,-0.98209,2.44979,-0.57495
-4.03226,0.55160,-1.17429,-1.70202,-0.98225,2.46284,-0.59255
-4.11290,0.54299,-1.17938,-1.70369,-0.98246,2.46931,-0.60156
-4.19355,0.53290,-1.18552,-1.70559,-0.98279,2.47684,-0.61230
-4.27419,0.52106,-1.19308,-1.70758,-0.98333,2.48559,-0.62515
-4.35484,0.51236,-1.19860,-1.70931,-0.98369,2.49198,-0.63480
-4.43548,0.50632,-1.20249,-1.71052,-0.98397,2.49638,-0.64158
-4.51613,0.49999,-1.20671,-1.71166,-0.98433,2.50098,-0.64878
-4.59677,0.49309,-1.21128,-1.71308,-0.98468,2.50595,-0.65673
-4.67742,0.48695,-1.21552,-1.71420,-0.98507,2.51036,-0.66390
-4.75806,0.48267,-1.21851,-1.71498,-0.98535,2.51341,-0.66894
-4.83871,0.47825,-1.22142,-1.71609,-0.98555,2.51655,-0.67418
-4.91935,0.47286,-1.22476,-1.71786,-0.98565,2.52037,-0.68065
-5.00000,0.46388,-1.22867,-1.72353,-0.98491,2.52667,-0.69155
-5.08065,0.45727,-1.23095,-1.72875,-0.98405,2.53128,-0.69971
-5.16129,0.45393,-1.23198,-1.73161,-0.98355,2.53358,-0.70386
-5.24194,0.45040,-1.23267,-1.73526,-0.98282,2.53602,-0.70828
-5.32258,0.44743,-1.23312,-1.73856,-0.98214,2.53806,-0.71202
-5.40323,0.44312,-1.23344,-1.74387,-0.98097,2.54101,-0.71749
-5.48387,0.42071,-1.22869,-1.78190,-0.97171,2.55610,-0.74660
-5.56452,0.41356,-1.22662,-1.79498,-0.96866,2.56083,-0.75618
-5.64516,0.40657,-1.22419,-1.80841,-0.96555,2.56541,-0.76565
-5.72581,0.39830,-1.22042,-1.82567,-0.96157,2.57077,-0.77703
-5.80645,0.39668,-1.21962,-1.82915,-0.96079,2.57182,-0.77929
-5.88710,0.39558,-1.21903,-1.83157,-0.96024,2.57253,-0.78083
-5.96774,0.39506,-1.21876,-1.83270,-0.95998,2.57286,-0.78155
-6.04839,0.39400,-1.21815,-1.83510,-0.95944,2.57354,-0.78303
-6.12903,0.39395,-1.21812,-1.83519,-0.95942,2.57357,-0.78309
-6.20968,0.39393,-1.21811,-1.83524,-0.95941,2.57358,-0.78312
-6.29032,0.39392,-1.21809,-1.83529,-0.95940,2.57359,-0.78314
-6.37097,0.39390,-1.21808,-1.83533,-0.95939,2.57361,-0.78317
-6.45161,0.39388,-1.21807,-1.83537,-0.95938,2.57362,-0.78319
-6.53226,0.39387,-1.21806,-1.83540,-0.95937,2.57362,-0.78321
-6.61290,0.39386,-1.21805,-1.83544,-0.95936,2.57363,-0.78323
-6.69355,0.39384,-1.21804,-1.83547,-0.95936,2.57364,-0.78324
-6.77419,0.39384,-1.21803,-1.83549,-0.95935,2.57364,-0.78325
-6.85484,0.39384,-1.21803,-1.83550,-0.95935,2.57365,-0.78325
-6.93548,0.39384,-1.21803,-1.83549,-0.95935,2.57364,-0.78325
-7.01613,0.39384,-1.21804,-1.83546,-0.95936,2.57364,-0.78324
-7.09677,0.39386,-1.21806,-1.83541,-0.95937,2.57363,-0.78323
-7.17742,0.39561,-1.22187,-1.82734,-0.96160,2.57252,-0.78081
-7.25806,0.39777,-1.22730,-1.81630,-0.96471,2.57114,-0.77782
-7.33871,0.39941,-1.23309,-1.80547,-0.96788,2.57010,-0.77560
-7.41935,0.40055,-1.23808,-1.79648,-0.97058,2.56937,-0.77405
-7.50000,0.40098,-1.24049,-1.79230,-0.97186,2.56909,-0.77346
-7.58065,0.40173,-1.24659,-1.78220,-0.97503,2.56862,-0.77248
-7.66129,0.40204,-1.25576,-1.76813,-0.97961,2.56843,-0.77211
-7.74194,0.40195,-1.26246,-1.75827,-0.98292,2.56850,-0.77228
-7.82258,0.40199,-1.27002,-1.74687,-0.98677,2.56850,-0.77229
-7.90323,0.40205,-1.27504,-1.73918,-0.98940,2.56846,-0.77224
-7.98387,0.40287,-1.28373,-1.72475,-0.99426,2.56795,-0.77119
-8.06452,0.40408,-1.29249,-1.70950,-0.99941,2.56718,-0.76960
-8.14516,0.40521,-1.30089,-1.69485,-1.00444,2.56646,-0.76813
-8.22581,0.40654,-1.30992,-1.67882,-1.00998,2.56561,-0.76638
-8.30645,0.40962,-1.32552,-1.64971,-1.02012,2.56364,-0.76234
-8.38710,0.41177,-1.33339,-1.63392,-1.02567,2.56224,-0.75950
-8.46774,0.41319,-1.33808,-1.62427,-1.02908,2.56132,-0.75763
-8.54839,0.41494,-1.34358,-1.61278,-1.03315,2.56018,-0.75533
-8.62903,0.41620,-1.34790,-1.60393,-1.03633,2.55936,-0.75369
-8.70968,0.42136,-1.36182,-1.57351,-1.04722,2.55598,-0.74695
-8.79032,0.42246,-1.36447,-1.56748,-1.04941,2.55525,-0.74551
-8.87097,0.42545,-1.37157,-1.55127,-1.05531,2.55327,-0.74164
-8.95161,0.42921,-1.37948,-1.53248,-1.06219,2.55077,-0.73679
-9.03226,0.43285,-1.38644,-1.51537,-1.06849,2.54834,-0.73214
-9.11290,0.43638,-1.39227,-1.50025,-1.07408,2.54597,-0.72763
-9.19355,0.44474,-1.40602,-1.46444,-1.08748,2.54032,-0.71710
-9.27419,0.44878,-1.41255,-1.44726,-1.09401,2.53757,-0.71209
-9.35484,0.45180,-1.41705,-1.43504,-1.09869,2.53550,-0.70835
-9.43548,0.45453,-1.42093,-1.42429,-1.10281,2.53363,-0.70499
-9.51613,0.45691,-1.42425,-1.41502,-1.10639,2.53199,-0.70208
-9.59677,0.46020,-1.42840,-1.40294,-1.11104,2.52972,-0.69807
-9.67742,0.46317,-1.43192,-1.39244,-1.11511,2.52767,-0.69448
-9.75806,0.46666,-1.43588,-1.38038,-1.11979,2.52524,-0.69027
-9.83871,0.46982,-1.43913,-1.37004,-1.12381,2.52304,-0.68649
-9.91935,0.47222,-1.44125,-1.36280,-1.12662,2.52136,-0.68363
-10.00000,0.47620,-1.44443,-1.35136,-1.13106,2.51856,-0.67891
-10.08065,0.47890,-1.44620,-1.34428,-1.13380,2.51665,-0.67572
-10.16129,0.48215,-1.44822,-1.33597,-1.13702,2.51436,-0.67191
-10.24194,0.48553,-1.45008,-1.32776,-1.14019,2.51196,-0.66796
-10.32258,0.48945,-1.45229,-1.31818,-1.14392,2.50917,-0.66342
-10.40323,0.49282,-1.45420,-1.30995,-1.14713,2.50676,-0.65954
-10.48387,0.49568,-1.45577,-1.30307,-1.14982,2.50471,-0.65628
-10.56452,0.49803,-1.45700,-1.29754,-1.15199,2.50302,-0.65360
-10.64516,0.50042,-1.45818,-1.29205,-1.15414,2.50130,-0.65090
-10.72581,0.50391,-1.45999,-1.28388,-1.15736,2.49878,-0.64696
-10.80645,0.50862,-1.46248,-1.27282,-1.16174,2.49537,-0.64169
-10.88710,0.51348,-1.46490,-1.26171,-1.16616,2.49184,-0.63630
-10.96774,0.51984,-1.46766,-1.24796,-1.17163,2.48719,-0.62933
-11.04839,0.52395,-1.46912,-1.23968,-1.17492,2.48417,-0.62485
-11.12903,0.52786,-1.47061,-1.23168,-1.17813,2.48129,-0.62064
-11.20968,0.53059,-1.47157,-1.22624,-1.18031,2.47928,-0.61771
-11.29032,0.53221,-1.47201,-1.22326,-1.18150,2.47808,-0.61597
-11.37097,0.53371,-1.47237,-1.22059,-1.18256,2.47697,-0.61438
-11.45161,0.53585,-1.47290,-1.21674,-1.18409,2.47538,-0.61211
-11.53226,0.53753,-1.47321,-1.21392,-1.18521,2.47413,-0.61032
-11.61290,0.54076,-1.47324,-1.20952,-1.18692,2.47173,-0.60691
-11.69355,0.54449,-1.47270,-1.20550,-1.18843,2.46894,-0.60300
-11.77419,0.54792,-1.47168,-1.20276,-1.18941,2.46637,-0.59941
-11.85484,0.55091,-1.47013,-1.20158,-1.18973,2.46412,-0.59629
-11.93548,0.55316,-1.46869,-1.20121,-1.18974,2.46243,-0.59396
-12.01613,0.55555,-1.46673,-1.20157,-1.18942,2.46063,-0.59149
-12.09677,0.55806,-1.46398,-1.20323,-1.18852,2.45873,-0.58889
-12.17742,0.56098,-1.46002,-1.20652,-1.18687,2.45651,-0.58588
-12.25806,0.56399,-1.45497,-1.21168,-1.18439,2.45422,-0.58278
-12.33871,0.56669,-1.44956,-1.21791,-1.18145,2.45217,-0.58002
-12.41935,0.56885,-1.44496,-1.22338,-1.17888,2.45051,-0.57780
-12.50000,0.57041,-1.44118,-1.22815,-1.17666,2.44931,-0.57620
-12.58065,0.57179,-1.43762,-1.23278,-1.17452,2.44825,-0.57479
-12.66129,0.57285,-1.43467,-1.23672,-1.17270,2.44744,-0.57370
-12.74194,0.57364,-1.43226,-1.24003,-1.17118,2.44683,-0.57289
-12.82258,0.57444,-1.42980,-1.24342,-1.16963,2.44621,-0.57207
-12.90323,0.57549,-1.42607,-1.24877,-1.16719,2.44540,-0.57099
-12.98387,0.57729,-1.41634,-1.26384,-1.16042,2.44400,-0.56911
-13.06452,0.57807,-1.41113,-1.27213,-1.15671,2.44339,-0.56828
-13.14516,0.57856,-1.40557,-1.28138,-1.15261,2.44300,-0.56774
-13.22581,0.57879,-1.39758,-1.29523,-1.14651,2.44281,-0.56743
-13.30645,0.57853,-1.39012,-1.30871,-1.14063,2.44298,-0.56760
-13.38710,0.57808,-1.38369,-1.32057,-1.13548,2.44331,-0.56797
-13.46774,0.57703,-1.37429,-1.33830,-1.12786,2.44409,-0.56890
-13.54839,0.57641,-1.36900,-1.34830,-1.12357,2.44455,-0.56945
-13.62903,0.57521,-1.36105,-1.36361,-1.11706,2.44545,-0.57056
-13.70968,0.57255,-1.35067,-1.38480,-1.10817,2.44745,-0.57309
-13.79032,0.56969,-1.34283,-1.40177,-1.10114,2.44960,-0.57584
-13.87097,0.56703,-1.33701,-1.41501,-1.09572,2.45161,-0.57845
-13.95161,0.56487,-1.33281,-1.42482,-1.09173,2.45324,-0.58057
-14.03226,0.56089,-1.32626,-1.44084,-1.08531,2.45624,-0.58453
-14.11290,0.55835,-1.32261,-1.45013,-1.08162,2.45814,-0.58707
-14.19355,0.55612,-1.31967,-1.45784,-1.07858,2.45982,-0.58932
-14.27419,0.55381,-1.31685,-1.46544,-1.07561,2.46155,-0.59166
-14.35484,0.55192,-1.31465,-1.47146,-1.07327,2.46297,-0.59359
-14.43548,0.55048,-1.31305,-1.47593,-1.07154,2.46404,-0.59505
-14.51613,0.54952,-1.31200,-1.47888,-1.07041,2.46476,-0.59604
-14.59677,0.54806,-1.31044,-1.48330,-1.06871,2.46586,-0.59754
-14.67742,0.54804,-1.31042,-1.48336,-1.06868,2.46587,-0.59756
-14.75806,0.54756,-1.30992,-1.48479,-1.06814,2.46623,-0.59805
-14.83871,0.54706,-1.30940,-1.48628,-1.06757,2.46660,-0.59857
-14.91935,0.54656,-1.30888,-1.48776,-1.06700,2.46698,-0.59908
-15.00000,0.54654,-1.30886,-1.48782,-1.06698,2.46699,-0.59911
diff --git a/python/examples/comparing_logs_example.py b/python/examples/log_analysis/comparing_logs_example.py
similarity index 100%
rename from python/examples/comparing_logs_example.py
rename to python/examples/log_analysis/comparing_logs_example.py
diff --git a/python/examples/log_analysis_example.py b/python/examples/log_analysis/log_analysis_example.py
similarity index 100%
rename from python/examples/log_analysis_example.py
rename to python/examples/log_analysis/log_analysis_example.py
diff --git a/python/examples/force_control_test.py b/python/examples/old_or_experimental/force_control/force_control_test.py
similarity index 100%
rename from python/examples/force_control_test.py
rename to python/examples/old_or_experimental/force_control/force_control_test.py
diff --git a/python/examples/old_or_experimental/force_mode_api.py b/python/examples/old_or_experimental/force_control/force_mode_api.py
similarity index 100%
rename from python/examples/old_or_experimental/force_mode_api.py
rename to python/examples/old_or_experimental/force_control/force_mode_api.py
diff --git a/python/examples/old_or_experimental/forcemode_example.py b/python/examples/old_or_experimental/force_control/forcemode_example.py
similarity index 100%
rename from python/examples/old_or_experimental/forcemode_example.py
rename to python/examples/old_or_experimental/force_control/forcemode_example.py
diff --git a/python/examples/point_force_control.py b/python/examples/old_or_experimental/force_control/point_force_control.py
similarity index 100%
rename from python/examples/point_force_control.py
rename to python/examples/old_or_experimental/force_control/point_force_control.py
diff --git a/python/examples/test_crocoddyl_opt_ctrl.py b/python/examples/old_or_experimental/test_crocoddyl_opt_ctrl.py
similarity index 100%
rename from python/examples/test_crocoddyl_opt_ctrl.py
rename to python/examples/old_or_experimental/test_crocoddyl_opt_ctrl.py
diff --git a/python/examples/casadi_ocp_collision_avoidance.py b/python/examples/optimal_control/casadi_ocp_collision_avoidance.py
similarity index 100%
rename from python/examples/casadi_ocp_collision_avoidance.py
rename to python/examples/optimal_control/casadi_ocp_collision_avoidance.py
diff --git a/python/examples/optimal_control/croco_ik_mpc.py b/python/examples/optimal_control/croco_ik_mpc.py
new file mode 100644
index 0000000000000000000000000000000000000000..5fc1ff99b65dc8efe9ab17237b92bbad79e67607
--- /dev/null
+++ b/python/examples/optimal_control/croco_ik_mpc.py
@@ -0,0 +1,52 @@
+import numpy as np
+from smc import getMinimalArgParser, getRobotFromArgs
+from smc.control.optimal_control.croco_mpc_point_to_point import (
+ CrocoIKMPC,
+)
+from smc.control.optimal_control.util import get_OCP_args
+from smc.control.cartesian_space import getClikArgs
+import pinocchio as pin
+import argcomplete
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
+ parser = getClikArgs(parser) # literally just for goal error
+ argcomplete.autocomplete(parser)
+ args = parser.parse_args()
+ return args
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = getRobotFromArgs(args)
+ # TODO: put this back for nicer demos
+ # Mgoal = defineGoalPointCLI()
+ T_w_goal = pin.SE3.Random()
+ if robot.robot_name == "yumi":
+ T_w_goal.rotation = np.eye(3)
+ T_w_goal_transform = pin.SE3.Identity()
+ T_w_goal_transform.translation[1] = 0.1
+ Mgoal_left = T_w_goal_transform.act(T_w_goal)
+ Mgoal_right = T_w_goal_transform.inverse().act(T_w_goal)
+ T_w_goal = (Mgoal_left, Mgoal_right)
+
+ if args.visualizer:
+ # TODO: document this somewhere
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
+
+ CrocoIKMPC(args, robot, T_w_goal)
+
+ print("final position:")
+ print(robot.T_w_e)
+
+ if args.real:
+ robot.stopRobot()
+
+ if args.visualizer:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot._log_manager.saveLog()
+ robot._log_manager.plotAllControlLoops()
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/optimal_control/croco_ik_ocp.py
similarity index 85%
rename from python/examples/crocoddyl_ocp_clik.py
rename to python/examples/optimal_control/croco_ik_ocp.py
index 32bef5cdce5cb68a873471b79826cd30a5c423af..9ca52152057d71bb23f3708c28afa3fb00be8af9 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/optimal_control/croco_ik_ocp.py
@@ -1,15 +1,13 @@
# PYTHON_ARGCOMPLETE_OK
from smc.control.optimal_control.util import get_OCP_args
from smc import getMinimalArgParser, getRobotFromArgs
-from smc.control.optimal_control import (
- createCrocoIKOCP,
- solveCrocoOCP,
+from smc.control.optimal_control.point_to_point_croco_ocp import (
+ SingleArmIKOCP,
)
from smc.control.joint_space import followKinematicJointTrajP
import pinocchio as pin
import numpy as np
-import crocoddyl
import argcomplete
@@ -26,11 +24,11 @@ if __name__ == "__main__":
robot = getRobotFromArgs(args)
# TODO: put this back for nicer demos
# Mgoal = robot.defineGoalPointCLI()
- Mgoal = pin.SE3.Random()
+ T_w_goal = pin.SE3.Random()
if args.visualizer:
# TODO document this somewhere
- robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
+ robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
# create and solve the optimal control problem of
# getting from current to goal end-effector position.
@@ -38,9 +36,9 @@ if __name__ == "__main__":
# while solver is a crocoddyl object containing a lot more information
# starting state
x0 = np.concatenate([robot.q, robot.v])
- problem = createCrocoIKOCP(args, robot, x0, Mgoal)
# this shouldn't really depend on x0 but i can't be bothered
- reference, solver = solveCrocoOCP(args, robot, problem, x0)
+ ocp = SingleArmIKOCP(args, robot, x0, T_w_goal)
+ reference = ocp.solveOCP(x0)
# NOTE: IF YOU PLOT SOMETHING OTHER THAN REAL-TIME PLOTTING FIRST IT BREAKS EVERYTHING
# if args.solver == "boxfddp":
diff --git a/python/examples/path_following_mpc.py b/python/examples/optimal_control/path_following_mpc.py
similarity index 100%
rename from python/examples/path_following_mpc.py
rename to python/examples/optimal_control/path_following_mpc.py
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
deleted file mode 100644
index c28926c91fc239e78ed6a53c7f3efb934cedcff4..0000000000000000000000000000000000000000
--- a/python/examples/path_in_pixels.csv
+++ /dev/null
@@ -1,46 +0,0 @@
-0.28846,0.66239
-0.29140,0.66108
-0.29287,0.66108
-0.29434,0.66108
-0.29581,0.66108
-0.31638,0.65586
-0.35166,0.65455
-0.37370,0.65194
-0.44278,0.65194
-0.46483,0.64803
-0.52215,0.64672
-0.52362,0.64541
-0.52656,0.64411
-0.52803,0.64411
-0.52950,0.64280
-0.53097,0.63367
-0.52950,0.62714
-0.52509,0.62061
-0.51921,0.61278
-0.50451,0.60364
-0.48834,0.59189
-0.46483,0.57231
-0.37076,0.50965
-0.32520,0.49268
-0.25171,0.46396
-0.23848,0.45090
-0.21644,0.43263
-0.20909,0.42741
-0.20615,0.42610
-0.20468,0.42479
-0.20321,0.42349
-0.20321,0.42349
-0.21056,0.41957
-0.23408,0.41566
-0.30168,0.41305
-0.32961,0.41174
-0.40310,0.40913
-0.42221,0.40652
-0.48394,0.40130
-0.54273,0.39607
-0.54860,0.39346
-0.55448,0.39216
-0.55595,0.39216
-0.55742,0.39216
-0.55889,0.39216
-0.55889,0.39216
diff --git a/python/examples/pin_contact3d.py b/python/examples/pinocchio_math_understanding/pin_contact3d.py
similarity index 100%
rename from python/examples/pin_contact3d.py
rename to python/examples/pinocchio_math_understanding/pin_contact3d.py
diff --git a/python/examples/dummy_cmds_pub.py b/python/examples/ros/dummy_cmds_pub.py
similarity index 100%
rename from python/examples/dummy_cmds_pub.py
rename to python/examples/ros/dummy_cmds_pub.py
diff --git a/python/examples/ros2_clik.py b/python/examples/ros/ros2_clik.py
similarity index 100%
rename from python/examples/ros2_clik.py
rename to python/examples/ros/ros2_clik.py
diff --git a/python/examples/smc_node.py b/python/examples/ros/smc_node.py
similarity index 100%
rename from python/examples/smc_node.py
rename to python/examples/ros/smc_node.py
diff --git a/python/examples/smc_yumi_challenge.py b/python/examples/ros/smc_yumi_challenge.py
similarity index 100%
rename from python/examples/smc_yumi_challenge.py
rename to python/examples/ros/smc_yumi_challenge.py
diff --git a/python/examples/camera_no_lag.py b/python/examples/vision/camera_no_lag.py
similarity index 100%
rename from python/examples/camera_no_lag.py
rename to python/examples/vision/camera_no_lag.py
diff --git a/python/smc/control/optimal_control/__init__.py b/python/smc/control/optimal_control/__init__.py
index 927eb07a605fa1c2d0e49f6b93c899affdd8bfc0..bd3883b08e418e5c1f5c1efb957bd0d1189b5dbc 100644
--- a/python/smc/control/optimal_control/__init__.py
+++ b/python/smc/control/optimal_control/__init__.py
@@ -6,5 +6,7 @@ import importlib.util
# print("you need to have pinocchio version 3.0.0 or greater to use pinocchio.casadi!")
# exit()
# from .create_pinocchio_casadi_ocp import *
-from .crocoddyl_mpc import *
-from .crocoddyl_optimal_control import *
+from .path_following_croco_ocp import *
+from .point_to_point_croco_ocp import *
+from .croco_mpc_path_following import *
+from .croco_mpc_point_to_point import *
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..867d3517e9464ef477ccf5719e1d2b10b066045b
--- /dev/null
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -0,0 +1,286 @@
+# TODO: make a bundle method which solves and immediately follows the traj.
+from smc.control.joint_space.joint_space_trajectory_following import (
+ followKinematicJointTrajP,
+)
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+
+import numpy as np
+import crocoddyl
+from argparse import Namespace
+import time
+import importlib.util
+import abc
+from typing import Any
+
+if importlib.util.find_spec("mim_solvers"):
+ try:
+ import mim_solvers
+ except ModuleNotFoundError:
+ print(
+ "mim_solvers installation is broken, rebuild and reinstall it if you want it"
+ )
+
+
+class CrocoOCP(abc.ABC):
+ """
+ CrocoOCP
+ ----------
+ General info:
+ - torque inputs are assumed - we already solve for state, which includes velocity commands, and we send that if the robot accepts vel cmds
+ State model : StateMultibody
+ I'm not even sure what else is there to choose lol.
+ Actuation model : ActuatioModelFull
+ We do underactuation via a constraint at the moment. This is solely
+ because coding up a proper underactuated model is annoying,
+ and dealing with this is a TODO for later. Having that said,
+ input constraints are necessary either way.
+ # TODO: consider ActuationModelFloatingBaseTpl for heron
+ # TODO: create a different actuation model (or whatever)
+ # for the mobile base - basically just remove the y movement in the base
+ # and update the corresponding derivates to 0
+ # there's python examples for this, ex. acrobot.
+ # you might want to implement the entire action model too idk what's really necessary here
+ Action model : DifferentialActionModelFreeFwdDynamics
+ We need to create an action model for running and terminal knots. The
+ forward dynamics (computed using ABA) are implemented
+ inside DifferentialActionModelFreeFwdDynamics.
+ """
+
+ def __init__(
+ self, args: Namespace, robot: AbstractRobotManager, x0: np.ndarray, goal
+ ):
+ # TODO: declare attributes here
+ self.args = args
+ self.solver_name = args.solver
+ self.robot = robot
+ self.x0 = x0
+ self.constructCrocoCostModels()
+ self.constructRegulationCosts()
+ self.constructStateLimits()
+ self.constructConstraints()
+ self.constructTaskObjectiveFunction(goal)
+ self.constructRunningModels()
+ self.problem = crocoddyl.ShootingProblem(
+ self.x0, self.runningModels, self.terminalModel
+ )
+ self.createSolver()
+
+ def constructCrocoCostModels(self):
+ self.state = crocoddyl.StateMultibody(self.robot.model)
+ # NOTE: atm we just set effort level in that direction to 0
+ self.actuation = crocoddyl.ActuationModelFull(self.state)
+
+ # we will be summing 4 different costs
+ # first 3 are for tracking, state and control regulation
+ self.runningCostModels = []
+ for _ in range(self.args.n_knots):
+ self.runningCostModels.append(crocoddyl.CostModelSum(self.state))
+ self.terminalCostModel = crocoddyl.CostModelSum(self.state)
+
+ def constructRegulationCosts(self):
+ # cost 1) u residual (actuator cost)
+ self.uResidual = crocoddyl.ResidualModelControl(self.state, self.state.nv)
+ self.uRegCost = crocoddyl.CostModelResidual(self.state, self.uResidual)
+ # cost 2) x residual (overall amount of movement)
+ self.xResidual = crocoddyl.ResidualModelState(
+ self.state, self.x0, self.state.nv
+ )
+ self.xRegCost = crocoddyl.CostModelResidual(self.state, self.xResidual)
+
+ # put these costs into the running costs
+ for i in range(self.args.n_knots):
+ self.runningCostModels[i].addCost("xReg", self.xRegCost, 1e-3)
+ self.runningCostModels[i].addCost("uReg", self.uRegCost, 1e-3)
+ # and add the terminal cost, which is the distance to the goal
+ # NOTE: shouldn't there be a final velocity = 0 here?
+ self.terminalCostModel.addCost("uReg", self.uRegCost, 1e3)
+
+ def constructStateLimits(self) -> None:
+ """
+ constructStateConstraints
+ -------------------------
+ """
+ # the 4th cost is for defining bounds via costs
+ # NOTE: could have gotten the same info from state.lb and state.ub.
+ # the first state is unlimited there idk what that means really,
+ # but the arm's base isn't doing a full rotation anyway, let alone 2 or more
+ self.xlb = np.concatenate(
+ [self.robot.model.lowerPositionLimit, -1 * self.robot.model.velocityLimit]
+ )
+ self.xub = np.concatenate(
+ [self.robot.model.upperPositionLimit, self.robot.model.velocityLimit]
+ )
+
+ # NOTE: in an ideal universe this is handled elsewhere
+ # we have no limits on the mobile base.
+ # the mobile base is a planar joint.
+ # since it is represented as [x,y,cos(theta),sin(theta)], there is no point
+ # to limiting cos(theta),sin(theta) even if we wanted limits,
+ # because we would then limit theta, or limit ct and st jointly.
+ # in any event, xlb and xub are 1 number too long --
+ # the residual has to be [x,y,theta] because it is in the tangent space -
+ # the difference between two points on a manifold in pinocchio is defined
+ # as the velocity which if parallel transported for 1 unit of "time"
+ # from one to point to the other.
+ # point activation input and the residual need to be of the same length obviously,
+ # and this should be 2 * model.nv the way things are defined here.
+
+ # TODO: replace this with subclass or whatever call
+ # to check if the robot has mobilebase interface
+ if (
+ self.robot.robot_name == "heron"
+ or self.robot.robot_name == "heronros"
+ or self.robot.robot_name == "mirros"
+ or self.robot.robot_name == "yumi"
+ ):
+ self.xlb = self.xlb[1:]
+ self.xub = self.xub[1:]
+
+ def constructConstraints(self) -> None:
+ if self.solver_name == "boxfddp":
+ self.stateConstraintsViaBarriersInObjectiveFunction()
+ if self.solver_name == "csqp":
+ self.boxInputConstraintsAsActualConstraints()
+
+ # NOTE: used by BoxFDDP
+ def stateConstraintsViaBarriersInObjectiveFunction(self) -> None:
+ bounds = crocoddyl.ActivationBounds(self.xlb, self.xub, 1.0)
+ xLimitResidual = crocoddyl.ResidualModelState(
+ self.state, self.x0, self.state.nv
+ )
+ xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
+
+ limitCost = crocoddyl.CostModelResidual(
+ self.state, xLimitActivation, xLimitResidual
+ )
+ for i in range(self.args.n_knots):
+ self.runningCostModels[i].addCost("limitCost", limitCost, 1e3)
+ self.terminalCostModel.addCost("limitCost", limitCost, 1e3)
+
+ # NOTE: used by csqp
+ def boxInputConstraintsAsActualConstraints(self) -> None:
+ # ConstraintModelManager just stores constraints
+ self.constraints = crocoddyl.ConstraintModelManager(
+ self.state, self.robot.model.nv
+ )
+ u_constraint = crocoddyl.ConstraintModelResidual(
+ self.state,
+ self.uResidual,
+ -1 * self.robot.model.effortLimit * 0.1,
+ self.robot.model.effortLimit * 0.1,
+ )
+ self.constraints.addConstraint("u_box_constraint", u_constraint)
+ x_constraint = crocoddyl.ConstraintModelResidual(
+ self.state, self.xResidual, self.xlb, self.xub
+ )
+ self.constraints.addConstraint("x_box_constraint", x_constraint)
+
+ def constructRunningModels(self) -> None:
+ self.runningModels = []
+ if self.solver_name == "boxfddp":
+ self.constructRunningModelsWithoutExplicitConstraints()
+ if self.solver_name == "csqp":
+ self.constructRunningModelsWithExplicitConstraints()
+
+ # NOTE: has input constraints, but reads them from model.effortlimit
+ # or whatever else
+ def constructRunningModelsWithoutExplicitConstraints(self) -> None:
+ for i in range(self.args.n_knots):
+ runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ self.state, self.actuation, self.runningCostModels[i]
+ ),
+ self.args.ocp_dt,
+ )
+ runningModel.u_lb = -1 * self.robot.model.effortLimit * 0.1
+ runningModel.u_ub = self.robot.model.effortLimit * 0.1
+ self.runningModels.append(runningModel)
+ self.terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ self.state, self.actuation, self.terminalCostModel
+ ),
+ 0.0,
+ )
+ self.terminalModel.u_lb = -1 * self.robot.model.effortLimit * 0.1
+ self.terminalModel.u_ub = self.robot.model.effortLimit * 0.1
+
+ def constructRunningModelsWithExplicitConstraints(self) -> None:
+ for i in range(self.args.n_knots):
+ runningModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ self.state,
+ self.actuation,
+ self.runningCostModels[i],
+ self.constraints,
+ ),
+ self.args.ocp_dt,
+ )
+ self.runningModels.append(runningModel)
+ self.terminalModel = crocoddyl.IntegratedActionModelEuler(
+ crocoddyl.DifferentialActionModelFreeInvDynamics(
+ self.state, self.actuation, self.terminalCostModel, self.constraints
+ ),
+ 0.0,
+ )
+
+
+ @abc.abstractmethod
+ def constructTaskObjectiveFunction(self, goal) -> None: ...
+
+ def getSolver(self) -> Any:
+ return self.solver
+
+ def createSolver(self) -> None:
+ if self.solver_name == "boxfddp":
+ self.createCrocoSolver()
+ if self.solver_name == "csqp":
+ self.createMimSolver()
+
+ # NOTE: used by boxfddp
+ def createCrocoSolver(self) -> None:
+ # just for reference can try
+ # solver = crocoddyl.SolverIpopt(problem)
+ # TODO: select other solvers from arguments
+ self.solver = crocoddyl.SolverBoxFDDP(self.problem)
+ # TODO: make these callbacks optional
+ self.solver.setCallbacks(
+ [crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()]
+ )
+
+ # NOTE: used by csqp
+ def createMimSolver(self) -> None:
+ # TODO try out the following solvers from mim_solvers:
+ # - csqp
+ # - stagewise qp
+ # and the solver
+ # both of these have generic inequalities you can put in.
+ # and both are basically QP versions of iLQR if i'm not wrong
+ # (i have no idea tho)
+ # solver = mim_solvers.SolverSQP(problem)
+ # TODO: select other solvers from arguments
+ self.solver = mim_solvers.SolverCSQP(self.problem)
+ # TODO: make these callbacks optional
+ self.solver.setCallbacks(
+ [mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()]
+ )
+
+ # this shouldn't really depend on x0 but i can't be bothered
+ def solveOCP(self, x0: np.ndarray) -> tuple[dict, Any]:
+ # run solve
+ # NOTE: there are some possible parameters here that I'm not using
+ xs = [x0] * (self.solver.problem.T + 1)
+ us = self.solver.problem.quasiStatic([x0] * self.solver.problem.T)
+
+ start = time.time()
+ self.solver.solve(xs, us, 500, False, 1e-9)
+ end = time.time()
+ print("solved in:", end - start, "seconds")
+
+ # solver.solve()
+ # get reference (state trajectory)
+ # we aren't using controls because we only have velocity inputs
+ xs = np.array(self.solver.xs)
+ qs = xs[:, : self.robot.model.nq]
+ vs = xs[:, self.robot.model.nq :]
+ reference = {"qs": qs, "vs": vs, "dt": self.args.ocp_dt}
+ return reference
diff --git a/python/smc/control/optimal_control/crocoddyl_mpc.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
similarity index 82%
rename from python/smc/control/optimal_control/crocoddyl_mpc.py
rename to python/smc/control/optimal_control/croco_mpc_path_following.py
index b415f3b1d55eeace1ac4f2339214994aad27aab9..7e63a890ca0ee3d8ad5b1a5b124eaea70f9c2a65 100644
--- a/python/smc/control/optimal_control/crocoddyl_mpc.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -1,13 +1,11 @@
from smc.robots.interfaces.single_arm_interface import SingleArmInterface
from smc.control.control_loop_manager import ControlLoopManager
from smc.multiprocessing.process_manager import ProcessManager
-from smc.control.optimal_control.crocoddyl_optimal_control import (
- createCrocoIKOCP,
- createCrocoEEPathFollowingOCP,
- createBaseAndEEPathFollowingOCP,
+from smc.control.optimal_control.path_following_croco_ocp import (
+ CrocoEEPathFollowingOCP,
+ BaseAndEEPathFollowingOCP,
)
-import pinocchio as pin
import crocoddyl
import numpy as np
from functools import partial
@@ -15,7 +13,12 @@ import types
import importlib.util
if importlib.util.find_spec("mim_solvers"):
- import mim_solvers
+ try:
+ import mim_solvers
+ except ModuleNotFoundError:
+ print(
+ "mim_solvers installation is broken, rebuild and reinstall it if you want it"
+ )
# TODO: put others here too
if importlib.util.find_spec("shapely"):
@@ -25,92 +28,13 @@ if importlib.util.find_spec("shapely"):
path2D_to_SE3,
)
-def CrocoIKMPCControlLoop(args, robot: SingleArmInterface, solver, T_w_goal, i, past_data):
- """
- CrocoIKMPCControlLoop
- ---------------------
- """
- breakFlag = False
- log_item = {}
- save_past_dict = {}
-
- # check for goal
- if robot.robot_name == "yumi":
- goal_left, goal_right = goal
- SEerror = robot.T_w_e.actInv(T_w_goal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < robot.args.goal_error:
- # print("Convergence achieved, reached destionation!")
- breakFlag = True
-
- # set initial state from sensor
- x0 = np.concatenate([robot.q, robot.v])
- solver.problem.x0 = x0
- # warmstart solver with previous solution
- xs_init = list(solver.xs[1:]) + [solver.xs[-1]]
- xs_init[0] = x0
- us_init = list(solver.us[1:]) + [solver.us[-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 :]
- robot.sendVelocityCommand(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]
-
- return breakFlag, save_past_dict, log_item
-
-
-def CrocoIKMPC(args, robot, T_w_goal, run=True):
- """
- IKMPC
- -----
- run mpc for a point-to-point inverse kinematics.
- note that the actual problem is solved on
- a dynamics level, and velocities we command
- are actually extracted from the state x(q,dq)
- """
- x0 = np.concatenate([robot.q, robot.v])
- problem = createCrocoIKOCP(args, robot, x0, T_w_goal)
- if args.solver == "boxfddp":
- solver = crocoddyl.SolverBoxFDDP(problem)
- if args.solver == "csqp":
- solver = mim_solvers.SolverCSQP(problem)
-
- # technically should be done in controlloop because now
- # it's solved 2 times before the first command,
- # but we don't have time for details rn
- xs_init = [x0] * (solver.problem.T + 1)
- us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
- solver.solve(xs_init, us_init, args.max_solver_iter)
-
- controlLoop = partial(CrocoIKMPCControlLoop, args, robot, solver, T_w_goal)
- 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),
- }
- save_past_dict = {}
- loop_manager = ControlLoopManager(
- robot, controlLoop, args, save_past_dict, log_item
- )
- if run:
- loop_manager.run()
- else:
- return loop_manager
-
def CrocoEndEffectorPathFollowingMPCControlLoop(
args,
robot: SingleArmInterface,
solver: crocoddyl.SolverBoxFDDP,
path_planner: ProcessManager,
- i,
+ t,
past_data,
):
"""
@@ -134,7 +58,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
# if it's the same 2D path
if type(path_planner) == types.FunctionType:
- pathSE3 = path_planner(T_w_e, i)
+ pathSE3 = path_planner(T_w_e, t)
else:
path_planner.sendCommand(p)
data = path_planner.getData()
@@ -162,7 +86,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(
# TODO: EVIL AND HAS TO BE REMOVED FROM HERE
if args.visualize_manipulator:
- if i % 20 == 0:
+ if t % 20 == 0:
robot.visualizer_manager.sendCommand({"frame_path": pathSE3})
x0 = np.concatenate([robot.q, robot.v])
diff --git a/python/smc/control/optimal_control/croco_mpc_point_to_point.py b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
new file mode 100644
index 0000000000000000000000000000000000000000..1f643a5b38837c0daf12a9d3c8914fd1365e9af2
--- /dev/null
+++ b/python/smc/control/optimal_control/croco_mpc_point_to_point.py
@@ -0,0 +1,92 @@
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.control_loop_manager import ControlLoopManager
+from smc.multiprocessing.process_manager import ProcessManager
+from smc.control.optimal_control.point_to_point_croco_ocp import SingleArmIKOCP
+
+import pinocchio as pin
+import crocoddyl
+import numpy as np
+from functools import partial
+import importlib.util
+
+if importlib.util.find_spec("mim_solvers"):
+ try:
+ import mim_solvers
+ except ModuleNotFoundError:
+ print(
+ "mim_solvers installation is broken, rebuild and reinstall it if you want it"
+ )
+
+
+def CrocoIKMPCControlLoop(args, robot: SingleArmInterface, solver, T_w_goal, _, __):
+ """
+ CrocoIKMPCControlLoop
+ ---------------------
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ SEerror = robot.T_w_e.actInv(T_w_goal)
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < robot.args.goal_error:
+ # print("Convergence achieved, reached destionation!")
+ breakFlag = True
+
+ # set initial state from sensor
+ x0 = np.concatenate([robot.q, robot.v])
+ solver.problem.x0 = x0
+ # warmstart solver with previous solution
+ xs_init = list(solver.xs[1:]) + [solver.xs[-1]]
+ xs_init[0] = x0
+ us_init = list(solver.us[1:]) + [solver.us[-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 :]
+ robot.sendVelocityCommand(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]
+
+ return breakFlag, save_past_dict, log_item
+
+
+def CrocoIKMPC(args, robot, T_w_goal, run=True):
+ """
+ IKMPC
+ -----
+ run mpc for a point-to-point inverse kinematics.
+ note that the actual problem is solved on
+ a dynamics level, and velocities we command
+ are actually extracted from the state x(q,dq)
+ """
+ x0 = np.concatenate([robot.q, robot.v])
+ ocp = SingleArmIKOCP(args, robot, x0, T_w_goal)
+ solver = ocp.getSolver()
+
+ # technically should be done in controlloop because now
+ # it's solved 2 times before the first command,
+ # but we don't have time for details rn
+ xs_init = [x0] * (solver.problem.T + 1)
+ us_init = solver.problem.quasiStatic([x0] * solver.problem.T)
+ solver.solve(xs_init, us_init, args.max_solver_iter)
+
+ controlLoop = partial(CrocoIKMPCControlLoop, args, robot, solver, T_w_goal)
+ 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),
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
diff --git a/python/smc/control/optimal_control/crocoddyl_optimal_control.py b/python/smc/control/optimal_control/crocoddyl_optimal_control.py
deleted file mode 100644
index 2b01dee7fd147461c8a1db17414457f8c778ead1..0000000000000000000000000000000000000000
--- a/python/smc/control/optimal_control/crocoddyl_optimal_control.py
+++ /dev/null
@@ -1,556 +0,0 @@
-from smc.control.joint_space.joint_space_trajectory_following import (
- followKinematicJointTrajP,
-)
-from smc.visualization.plotters import plotFromDict
-from smc import getMinimalArgParser
-from smc.robots.robotmanager_abstract import AbstractRobotManager
-from smc.robots.interfaces.single_arm_interface import SingleArmInterface
-from smc.robots.interfaces.dual_arm_interface import DualArmInterface
-
-import numpy as np
-import pinocchio as pin
-import crocoddyl
-from argparse import Namespace
-import example_robot_data
-import time
-import importlib.util
-import abc
-
-if importlib.util.find_spec("mim_solvers"):
- import mim_solvers
-
-
-class CrocoOCP(abc.ABC):
- """
- CrocoOCP
- ----------
- General info:
- - torque inputs are assumed - we already solve for state, which includes velocity commands, and we send that if the robot accepts vel cmds
- State model : StateMultibody
- I'm not even sure what else is there to choose lol.
- Actuation model : ActuatioModelFull
- We do underactuation via a constraint at the moment. This is solely
- because coding up a proper underactuated model is annoying,
- and dealing with this is a TODO for later. Having that said,
- input constraints are necessary either way.
- # TODO: consider ActuationModelFloatingBaseTpl for heron
- # TODO: create a different actuation model (or whatever)
- # for the mobile base - basically just remove the y movement in the base
- # and update the corresponding derivates to 0
- # there's python examples for this, ex. acrobot.
- # you might want to implement the entire action model too idk what's really necessary here
- Action model : DifferentialActionModelFreeFwdDynamics
- We need to create an action model for running and terminal knots. The
- forward dynamics (computed using ABA) are implemented
- inside DifferentialActionModelFreeFwdDynamics.
- """
-
- def __init__(
- self, args: Namespace, robot: AbstractRobotManager, x0: np.ndarray, solver: str
- ):
- # TODO: declare attributes here
- self.x0 = x0
- self.constructCrocoModels()
- self.constructRegulationCosts()
- self.constructStateConstraints()
- self.constructInputConstraints(solver)
- self.problem = crocoddyl.ShootingProblem(
- self.x0, [self.runningModel] * args.n_knots, self.terminalModel
- )
-
- def constructCrocoModels(self):
- self.state = crocoddyl.StateMultibody(self.robot.model)
- # NOTE: atm we just set effort level in that direction to 0
- self.actuation = crocoddyl.ActuationModelFull(self.state)
-
- # we will be summing 4 different costs
- # first 3 are for tracking, state and control regulation
- self.runningCostModel = crocoddyl.CostModelSum(self.state)
- self.terminalCostModel = crocoddyl.CostModelSum(self.state)
-
- def constructRegulationCosts(self):
- # cost 1) u residual (actuator cost)
- self.uResidual = crocoddyl.ResidualModelControl(self.state, self.state.nv)
- self.uRegCost = crocoddyl.CostModelResidual(self.state, self.uResidual)
- # cost 2) x residual (overall amount of movement)
- self.xResidual = crocoddyl.ResidualModelState(
- self.state, self.x0, self.state.nv
- )
- self.xRegCost = crocoddyl.CostModelResidual(self.state, self.xResidual)
-
- # put these costs into the running costs
- self.runningCostModel.addCost("xReg", self.xRegCost, 1e-3)
- self.runningCostModel.addCost("uReg", self.uRegCost, 1e-3)
- # and add the terminal cost, which is the distance to the goal
- # NOTE: shouldn't there be a final velocity = 0 here?
- self.terminalCostModel.addCost("uReg", self.uRegCost, 1e3)
-
- def constructStateConstraints(self) -> None:
- """
- constructStateConstraints
- -------------------------
- """
- # the 4th cost is for defining bounds via costs
- # NOTE: could have gotten the same info from state.lb and state.ub.
- # the first state is unlimited there idk what that means really,
- # but the arm's base isn't doing a full rotation anyway, let alone 2 or more
- xlb = np.concatenate(
- [self.robot.model.lowerPositionLimit, -1 * self.robot.model.velocityLimit]
- )
- xub = np.concatenate(
- [self.robot.model.upperPositionLimit, self.robot.model.velocityLimit]
- )
-
- # NOTE: in an ideal universe this is handled elsewhere
- # we have no limits on the mobile base.
- # the mobile base is a planar joint.
- # since it is represented as [x,y,cos(theta),sin(theta)], there is no point
- # to limiting cos(theta),sin(theta) even if we wanted limits,
- # because we would then limit theta, or limit ct and st jointly.
- # in any event, xlb and xub are 1 number too long --
- # the residual has to be [x,y,theta] because it is in the tangent space -
- # the difference between two points on a manifold in pinocchio is defined
- # as the velocity which if parallel transported for 1 unit of "time"
- # from one to point to the other.
- # point activation input and the residual need to be of the same length obviously,
- # and this should be 2 * model.nv the way things are defined here.
-
- # TODO: replace this with subclass or whatever call
- # to check if the robot has mobilebase interface
- if (
- self.robot.robot_name == "heron"
- or self.robot.robot_name == "heronros"
- or self.robot.robot_name == "mirros"
- or self.robot.robot_name == "yumi"
- ):
- xlb = xlb[1:]
- xub = xub[1:]
-
- def constructInputConstraints(self) -> None:
- if solver == "boxFDDP":
- self.inputConstraintsViaBarriersInObjectiveFunction()
- if solver == "csqp":
- self.boxInputConstraintsAsActualConstraints()
-
- # NOTE: used by BoxFDDP
- def inputConstraintsViaBarriersInObjectiveFunction(self) -> None:
- bounds = crocoddyl.ActivationBounds(xlb, xub, 1.0)
- xLimitResidual = crocoddyl.ResidualModelState(
- self.state, self.x0, self.state.nv
- )
- xLimitActivation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
-
- limitCost = crocoddyl.CostModelResidual(
- self.state, xLimitActivation, xLimitResidual
- )
- self.runningCostModel.addCost("limitCost", limitCost, 1e3)
- self.terminalCostModel.addCost("limitCost", limitCost, 1e3)
-
- self.runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- self.state, self.actuation, self.runningCostModel
- ),
- args.ocp_dt,
- )
- self.runningModel.u_lb = -1 * self.robot.model.effortLimit * 0.1
- self.runningModel.u_ub = self.robot.model.effortLimit * 0.1
- self.terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- self.state, self.actuation, self.terminalCostModel
- ),
- 0.0,
- )
- self.terminalModel.u_lb = -1 * self.robot.model.effortLimit * 0.1
- self.terminalModel.u_ub = self.robot.model.effortLimit * 0.1
-
- # NOTE: used by csqp
- def boxInputConstraintsAsActualConstraints(self) -> None:
- # ConstraintModelManager just stores constraints
- constraints = crocoddyl.ConstraintModelManager(self.state, self.robot.model.nv)
- u_constraint = crocoddyl.ConstraintModelResidual(
- self.state,
- self.uResidual,
- -1 * self.robot.model.effortLimit * 0.1,
- self.robot.model.effortLimit * 0.1,
- )
- constraints.addConstraint("u_box_constraint", u_constraint)
- x_constraint = crocoddyl.ConstraintModelResidual(
- self.state, self.xResidual, xlb, xub
- )
- constraints.addConstraint("x_box_constraint", x_constraint)
- self.runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- self.state, self.actuation, self.runningCostModel, constraints
- ),
- args.ocp_dt,
- )
- self.terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- self.state, self.actuation, self.terminalCostModel, constraints
- ),
- 0.0,
- )
-
- @abc.abstractmethod
- def constructTaskObjectiveFunction(self, goal) -> None: ...
-
- # NOTE: used by boxfddp
- def createCrocoSolver(self, args, robot, x0):
- # just for reference can try
- # solver = crocoddyl.SolverIpopt(problem)
- self.solver = crocoddyl.SolverBoxFDDP(self.problem)
- # TODO: make these callbacks optional
- self.solver.setCallbacks(
- [crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()]
- )
-
- # NOTE: used by csqp
- def createMimSolver(self, args, robot, x0):
- # TODO try out the following solvers from mim_solvers:
- # - csqp
- # - stagewise qp
- # and the solver
- # both of these have generic inequalities you can put in.
- # and both are basically QP versions of iLQR if i'm not wrong
- # (i have no idea tho)
- # solver = mim_solvers.SolverSQP(problem)
- self.solver = mim_solvers.SolverCSQP(self.problem)
- # TODO: make these callbacks optional
- self.solver.setCallbacks(
- [mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()]
- )
-
- # this shouldn't really depend on x0 but i can't be bothered
- def solveOCP(self, args, robot, x0):
- # run solve
- # NOTE: there are some possible parameters here that I'm not using
- xs = [x0] * (self.solver.problem.T + 1)
- us = self.solver.problem.quasiStatic([x0] * self.solver.problem.T)
-
- start = time.time()
- solver.solve(xs, us, 500, False, 1e-9)
- end = time.time()
- print("solved in:", end - start, "seconds")
-
- # solver.solve()
- # get reference (state trajectory)
- # we aren't using controls because we only have velocity inputs
- xs = np.array(solver.xs)
- qs = xs[:, : robot.model.nq]
- vs = xs[:, robot.model.nq :]
- reference = {"qs": qs, "vs": vs, "dt": args.ocp_dt}
- return reference, solver
-
-
-class SingleArmIKOCP(CrocoOCP):
- def __init__(
- args: Namespace, robot: SingleArmInterface, x0: np.ndarray, T_w_goal: pin.SE3
- ):
- super().__init__(args, robot, x0)
- self.constructTaskObjectiveFunction(T_w_goal)
-
- def constructTaskObjectiveFunction(self, T_w_goal):
- # cost 3) distance to goal -> SE(3) error
- # TODO: make this follow a path.
- # to do so, you need to implement a residualmodel for that in crocoddyl.
- # there's an example of exending crocoddyl in colmpc repo
- # (look at that to see how to compile, make python bindings etc)
- framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
- self.state,
- # TODO: check if this is the frame we actually want (ee tip)
- # the id is an integer and that's what api wants
- robot.model.getFrameId("tool0"),
- T_w_goal.copy(),
- self.state.nv,
- )
- goalTrackingCost = crocoddyl.CostModelResidual(
- self.state, framePlacementResidual
- )
- # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
- # because that's a part of the state, and we don't know final joint positions)
- frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
- self.state,
- robot.model.getFrameId("tool0"),
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD,
- )
- frameVelocityCost = crocoddyl.CostModelResidual(
- self.state, frameVelocityResidual
- )
- self.runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
- self.terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
- self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
-
-
-def DualArmIKOCP(CrocoOCP):
- def __init__(
- args: Namespace, robot: DualArmInterface, x0: np.ndarray, goal: pin.SE3
- ):
- super().__init__(args, robot, x0)
-
- def constructTaskObjectiveFunction(self, goal):
- T_w_lgoal, T_w_rgoal = goal
- framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.model.l_ee_frame_id,
- T_w_lgoal.copy(),
- self.state.nv,
- )
- framePlacementResidual_r = crocoddyl.ResidualModelFramePlacement(
- self.state,
- self.robot.model.r_ee_frame_id,
- T_w_rgoal.copy(),
- self.state.nv,
- )
- goalTrackingCost_l = crocoddyl.CostModelResidual(
- self.state, framePlacementResidual_l
- )
- goalTrackingCost_r = crocoddyl.CostModelResidual(
- self.state, framePlacementResidual_r
- )
- frameVelocityResidual_l = crocoddyl.ResidualModelFrameVelocity(
- self.state,
- self.robot.model.l_ee_frame_id,
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD,
- )
- frameVelocityResidual_r = crocoddyl.ResidualModelFrameVelocity(
- self.state,
- self.robot.model.r_ee_frame_id,
- pin.Motion(np.zeros(6)),
- pin.ReferenceFrame.WORLD,
- )
- frameVelocityCost_l = crocoddyl.CostModelResidual(
- self.state, frameVelocityResidual_l
- )
- frameVelocityCost_r = crocoddyl.CostModelResidual(
- self.state, frameVelocityResidual_r
- )
- self.runningCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
- self.runningCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
- self.terminalCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
- self.terminalCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
- self.terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
- self.terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
-
-
-class CrocoEEPathFollowingOCP(CrocoOCP):
- """
- createCrocoEEPathFollowingOCP
- -------------------------------
- creates a path following problem with a single end-effector reference.
- it is instantiated to just to stay at the current position.
- NOTE: the path MUST be time indexed with the SAME time used between the knots
- """
-
- def __init__(self, args: Namespace, robot: SingleArmInterface, x0: np.ndarray):
- super().__init__(self, args, robot, x0)
-
- def constructTaskObjectiveFunction(self, goal):
- T_w_e = robot.T_w_e
- path = [T_w_e] * args.n_knots
-
- # TODO: you need to find a way to make this mesh
- # with the generic construction.
- # the only difference is that we construct SLIGHTLY DIFFERENT running models here
- # instead of multiplying the same one
- def constructPathFollowingRunningModels(self):
- runningModels = []
- for i in range(args.n_knots):
- runningCostModel = crocoddyl.CostModelSum(state)
- runningCostModel.addCost("xReg", xRegCost, 1e-3)
- runningCostModel.addCost("uReg", uRegCost, 1e-3)
- if args.solver == "boxfddp":
- runningCostModel.addCost("limitCost", limitCost, 1e3)
- framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
- state,
- robot.model.getFrameId("tool0"),
- path[i], # this better be done with the same time steps as the knots
- # NOTE: this should be done outside of this function to clarity
- state.nv,
- )
- goalTrackingCost = crocoddyl.CostModelResidual(
- state, framePlacementResidual
- )
- runningCostModel.addCost("gripperPose" + str(i), goalTrackingCost, 1e2)
- # runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
-
- if args.solver == "boxfddp":
- runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, runningCostModel
- ),
- args.ocp_dt,
- )
- runningModel.u_lb = -1 * robot.model.effortLimit * 0.1
- runningModel.u_ub = robot.model.effortLimit * 0.1
- if args.solver == "csqp":
- runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, runningCostModel, constraints
- ),
- args.ocp_dt,
- )
- runningModels.append(runningModel)
-
- # terminal model
- if args.solver == "boxfddp":
- terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, terminalCostModel
- ),
- 0.0,
- )
- terminalModel.u_lb = -1 * robot.model.effortLimit * 0.1
- terminalModel.u_ub = robot.model.effortLimit * 0.1
- if args.solver == "csqp":
- terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, terminalCostModel, constraints
- ),
- 0.0,
- )
-
- terminalCostModel.addCost(
- "gripperPose" + str(args.n_knots), goalTrackingCost, 1e2
- )
- # terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
-
- # now we define the problem
- problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
- return problem
-
-
-def createBaseAndEEPathFollowingOCP(args, robot: AbstractRobotManager, x0):
- """
- createBaseAndEEPathFollowingOCP
- -------------------------------
- creates a path following problem.
- it is instantiated to just to stay at the current position.
- NOTE: the path MUST be time indexed with the SAME time used between the knots
- """
- if robot.robot_name != "yumi":
- T_w_e = robot.getT_w_e()
- path_ee = [T_w_e] * args.n_knots
- else:
- T_w_e_left, T_w_e_right = robot.getT_w_e()
- path_ee = [T_w_e_left] * args.n_knots
- # TODO: have a different reference for each arm
- path_base = [np.append(x0[:2], 0.0)] * args.n_knots
-
- # TODO: MAKE A RUNNING MODEL FUNCTION HERE WHICH
- # PLAYS BALL WITH GENERAL CONSTRUCTION
- runningModels = []
- for i in range(args.n_knots):
- runningCostModel = crocoddyl.CostModelSum(state)
- runningCostModel.addCost("xReg", xRegCost, 1e-3)
- runningCostModel.addCost("uReg", uRegCost, 1e-3)
- if args.solver == "boxfddp":
- runningCostModel.addCost("limitCost", limitCost, 1e3)
-
- ##########################
- # single arm reference #
- ##########################
- if robot.robot_name != "yumi":
- eePoseResidual = crocoddyl.ResidualModelFramePlacement(
- state,
- robot.model.getFrameId("tool0"),
- path_ee[i], # this better be done with the same time steps as the knots
- # NOTE: this should be done outside of this function to clarity
- state.nv,
- )
- eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
- runningCostModel.addCost(
- "ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost
- )
- #########################
- # dual arm references #
- #########################
- else:
- l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
- state,
- robot.model.l_ee_frame_id,
- # MASSIVE TODO: actually put in reference for left arm
- path_ee[i],
- state.nv,
- )
- l_eeTrackingCost = crocoddyl.CostModelResidual(state, l_eePoseResidual)
- runningCostModel.addCost(
- "l_ee_pose" + str(i), l_eeTrackingCost, args.ee_pose_cost
- )
- r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
- state,
- robot.model.r_ee_frame_id,
- # MASSIVE TODO: actually put in reference for left arm
- path_ee[i],
- state.nv,
- )
- r_eeTrackingCost = crocoddyl.CostModelResidual(state, r_eePoseResidual)
- runningCostModel.addCost(
- "r_ee_pose" + str(i), r_eeTrackingCost, args.ee_pose_cost
- )
-
- baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
- state, robot.model.getFrameId("mobile_base"), path_base[i], state.nv
- )
- baseTrackingCost = crocoddyl.CostModelResidual(state, baseTranslationResidual)
- runningCostModel.addCost(
- "base_translation" + str(i), baseTrackingCost, args.base_translation_cost
- )
-
- if args.solver == "boxfddp":
- runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, runningCostModel
- ),
- args.ocp_dt,
- )
- runningModel.u_lb = -1 * robot.model.effortLimit * 0.1
- runningModel.u_ub = robot.model.effortLimit * 0.1
- if args.solver == "csqp":
- runningModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, runningCostModel, constraints
- ),
- args.ocp_dt,
- )
- runningModels.append(runningModel)
-
- # terminal model
- if args.solver == "boxfddp":
- terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, terminalCostModel
- ),
- 0.0,
- )
- terminalModel.u_lb = -1 * robot.model.effortLimit * 0.1
- terminalModel.u_ub = robot.model.effortLimit * 0.1
- if args.solver == "csqp":
- terminalModel = crocoddyl.IntegratedActionModelEuler(
- crocoddyl.DifferentialActionModelFreeInvDynamics(
- state, actuation, terminalCostModel, constraints
- ),
- 0.0,
- )
-
- if robot.robot_name != "yumi":
- terminalCostModel.addCost(
- "ee_pose" + str(args.n_knots), eeTrackingCost, args.ee_pose_cost
- )
- else:
- terminalCostModel.addCost(
- "l_ee_pose" + str(args.n_knots), l_eeTrackingCost, args.ee_pose_cost
- )
- terminalCostModel.addCost(
- "r_ee_pose" + str(args.n_knots), r_eeTrackingCost, args.ee_pose_cost
- )
- terminalCostModel.addCost(
- "base_translation" + str(args.n_knots),
- baseTrackingCost,
- args.base_translation_cost,
- )
-
- # now we define the problem
- problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
- return problem
diff --git a/python/smc/control/optimal_control/path_following_croco_ocp.py b/python/smc/control/optimal_control/path_following_croco_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..26fdae9f5eb727b0404740447c660133da41c0aa
--- /dev/null
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -0,0 +1,163 @@
+# TODO: make a bundle method which solves and immediately follows the traj.
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.robots.robotmanager_abstract import AbstractRobotManager
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+
+import numpy as np
+import crocoddyl
+from argparse import Namespace
+
+
+class CrocoEEPathFollowingOCP(CrocoOCP):
+ """
+ createCrocoEEPathFollowingOCP
+ -------------------------------
+ creates a path following problem with a single end-effector reference.
+ it is instantiated to just to stay at the current position.
+ NOTE: the path MUST be time indexed with the SAME time used between the knots
+ """
+
+ def __init__(self, args: Namespace, robot: SingleArmInterface, x0: np.ndarray):
+ goal = None
+ super().__init__(args, robot, x0, goal)
+
+ def constructTaskObjectiveFunction(self, goal):
+ T_w_e = self.robot.T_w_e
+ path = [T_w_e] * self.args.n_knots
+
+ for i in range(self.args.n_knots):
+ framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.ee_frame_id,
+ path[i], # this better be done with the same time steps as the knots
+ # NOTE: this should be done outside of this function to clarity
+ self.state.nv,
+ )
+ goalTrackingCost = crocoddyl.CostModelResidual(
+ self.state, framePlacementResidual
+ )
+ self.runningCostModels[i].addCost(
+ "gripperPose" + str(i), goalTrackingCost, 1e2
+ )
+ # runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+
+ self.terminalCostModel.addCost(
+ "gripperPose" + str(self.args.n_knots), goalTrackingCost, 1e2
+ )
+ # terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+
+
+class BaseAndEEPathFollowingOCP(CrocoOCP):
+ """
+ createBaseAndEEPathFollowingOCP
+ -------------------------------
+ creates a path following problem.
+ it is instantiated to just to stay at the current position.
+ NOTE: the path MUST be time indexed with the SAME time used between the knots
+ """
+
+ def __init__(self, args, robot: AbstractRobotManager, x0):
+ goal = None
+ super().__init__(args, robot, x0, goal)
+
+ def constructTaskObjectiveFunction(self, goal) -> None:
+ T_w_e = self.robot.T_w_e
+ path_ee = [T_w_e] * self.args.n_knots
+ path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
+
+ for i in range(self.args.n_knots):
+ eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.model.ee_frame_id,
+ path_ee[i], # this better be done with the same time steps as the knots
+ # NOTE: this should be done outside of this function to clarity
+ self.state.nv,
+ )
+ eeTrackingCost = crocoddyl.CostModelResidual(self.state, eePoseResidual)
+ self.runningCostModels[i].addCost(
+ "ee_pose" + str(i), eeTrackingCost, self.args.ee_pose_cost
+ )
+
+ baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
+ self.state, self.robot.model.base_frame_id, path_base[i], self.state.nv
+ )
+ baseTrackingCost = crocoddyl.CostModelResidual(
+ self.state, baseTranslationResidual
+ )
+ self.runningCostModels[i].addCost(
+ "base_translation" + str(i),
+ baseTrackingCost,
+ self.args.base_translation_cost,
+ )
+
+ self.terminalCostModel.addCost(
+ "ee_pose" + str(self.args.n_knots), eeTrackingCost, self.args.ee_pose_cost
+ )
+ self.terminalCostModel.addCost(
+ "base_translation" + str(self.args.n_knots),
+ baseTrackingCost,
+ self.args.base_translation_cost,
+ )
+
+
+class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
+ def __init__(self, args, robot: DualArmInterface, x0):
+ goal = None
+ super().__init__(args, robot, x0, goal)
+
+ def constructTaskObjectiveFunction(self, goal) -> None:
+ T_w_e_left, T_w_e_right = self.robot.getT_w_e
+ path_ee_left = [T_w_e_left] * self.args.n_knots
+ path_ee_right = [T_w_e_right] * self.args.n_knots
+ path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
+
+ for i in range(self.args.n_knots):
+ l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.model.l_ee_frame_id,
+ # MASSIVE TODO: actually put in reference for left arm
+ path_ee_left[i],
+ self.state.nv,
+ )
+ l_eeTrackingCost = crocoddyl.CostModelResidual(self.state, l_eePoseResidual)
+ self.runningCostModels[i].addCost(
+ "l_ee_pose" + str(i), l_eeTrackingCost, self.args.ee_pose_cost
+ )
+ r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.model.r_ee_frame_id,
+ # MASSIVE TODO: actually put in reference for left arm
+ path_ee_right[i],
+ self.state.nv,
+ )
+ r_eeTrackingCost = crocoddyl.CostModelResidual(self.state, r_eePoseResidual)
+ self.runningCostModels[i].addCost(
+ "r_ee_pose" + str(i), r_eeTrackingCost, self.args.ee_pose_cost
+ )
+ baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
+ self.state, self.robot.model.base_frame_id, path_base[i], self.state.nv
+ )
+ baseTrackingCost = crocoddyl.CostModelResidual(
+ self.state, baseTranslationResidual
+ )
+ self.runningCostModels[i].addCost(
+ "base_translation" + str(i),
+ baseTrackingCost,
+ self.args.base_translation_cost,
+ )
+ self.terminalCostModel.addCost(
+ "l_ee_pose" + str(self.args.n_knots),
+ l_eeTrackingCost,
+ self.args.ee_pose_cost,
+ )
+ self.terminalCostModel.addCost(
+ "r_ee_pose" + str(self.args.n_knots),
+ r_eeTrackingCost,
+ self.args.ee_pose_cost,
+ )
+ self.terminalCostModel.addCost(
+ "base_translation" + str(self.args.n_knots),
+ baseTrackingCost,
+ self.args.base_translation_cost,
+ )
diff --git a/python/smc/control/optimal_control/point_to_point_croco_ocp.py b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..39c6e19bc4c3c2901c5ce751e6c286c2aa109ba5
--- /dev/null
+++ b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
@@ -0,0 +1,107 @@
+from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+
+import numpy as np
+import pinocchio as pin
+import crocoddyl
+from argparse import Namespace
+
+
+class SingleArmIKOCP(CrocoOCP):
+ def __init__(
+ self,
+ args: Namespace,
+ robot: SingleArmInterface,
+ x0: np.ndarray,
+ T_w_goal: pin.SE3,
+ ):
+ super().__init__(args, robot, x0, T_w_goal)
+ self.constructTaskObjectiveFunction(T_w_goal)
+
+ def constructTaskObjectiveFunction(self, goal):
+ T_w_goal = goal
+ # cost 3) distance to goal -> SE(3) error
+ # TODO: make this follow a path.
+ # to do so, you need to implement a residualmodel for that in crocoddyl.
+ # there's an example of exending crocoddyl in colmpc repo
+ # (look at that to see how to compile, make python bindings etc)
+ framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ # TODO: check if this is the frame we actually want (ee tip)
+ # the id is an integer and that's what api wants
+ self.robot.model.getFrameId("tool0"),
+ T_w_goal.copy(),
+ self.state.nv,
+ )
+ goalTrackingCost = crocoddyl.CostModelResidual(
+ self.state, framePlacementResidual
+ )
+ # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
+ # because that's a part of the state, and we don't know final joint positions)
+ frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
+ self.state,
+ self.robot.model.getFrameId("tool0"),
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD,
+ )
+ frameVelocityCost = crocoddyl.CostModelResidual(
+ self.state, frameVelocityResidual
+ )
+ for i in range(self.args.n_knots):
+ self.runningCostModels[i].addCost("gripperPose", goalTrackingCost, 1e2)
+ self.terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
+
+
+class DualArmIKOCP(CrocoOCP):
+ def __init__(
+ self, args: Namespace, robot: DualArmInterface, x0: np.ndarray, goal: pin.SE3
+ ):
+ super().__init__(args, robot, x0, goal)
+
+ def constructTaskObjectiveFunction(self, goal):
+ T_w_lgoal, T_w_rgoal = goal
+ framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.model.l_ee_frame_id,
+ T_w_lgoal.copy(),
+ self.state.nv,
+ )
+ framePlacementResidual_r = crocoddyl.ResidualModelFramePlacement(
+ self.state,
+ self.robot.model.r_ee_frame_id,
+ T_w_rgoal.copy(),
+ self.state.nv,
+ )
+ goalTrackingCost_l = crocoddyl.CostModelResidual(
+ self.state, framePlacementResidual_l
+ )
+ goalTrackingCost_r = crocoddyl.CostModelResidual(
+ self.state, framePlacementResidual_r
+ )
+ frameVelocityResidual_l = crocoddyl.ResidualModelFrameVelocity(
+ self.state,
+ self.robot.model.l_ee_frame_id,
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD,
+ )
+ frameVelocityResidual_r = crocoddyl.ResidualModelFrameVelocity(
+ self.state,
+ self.robot.model.r_ee_frame_id,
+ pin.Motion(np.zeros(6)),
+ pin.ReferenceFrame.WORLD,
+ )
+ frameVelocityCost_l = crocoddyl.CostModelResidual(
+ self.state, frameVelocityResidual_l
+ )
+ frameVelocityCost_r = crocoddyl.CostModelResidual(
+ self.state, frameVelocityResidual_r
+ )
+ for i in range(self.args.n_knots):
+ self.runningCostModels[i].addCost("gripperPose_l", goalTrackingCost_l, 1e2)
+ self.runningCostModels[i].addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+ self.terminalCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
+ self.terminalCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+ self.terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
+ self.terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index 5d7f652cc10cb866bc9367b26353664560d19d39..ba9768a545ac675a39896b902be43a411e64203c 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -262,7 +262,7 @@ def get_model():
model.jointPlacements[5] = wrist_2_se3
model.jointPlacements[6] = wrist_3_se3
# TODO: fix where the fingers end up by setting a better position here (or maybe not here idk)
- # model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
+ model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
data = pin.Data(model)
return model, collision_model, visual_model, data