OCP Croco Class #112

ArthurH91 · 2024-12-11T17:54:06Z

Hello,

Intro

Here's a draft PR on the OCP base class.

After a discussion with @MaximilienNaveau , we agreed that having only crocoddyl might restrict some users. So we decided to have the OCPBase class staying as it is right now, and having a daughter class OCPCrocoBase, which is a simple task to reach a target with the end effector, as a baseline.

In this draft, I'd like to discuss this with y'all. Do you agree with this reasoning?

What's in the PR

changes on absolute paths for files relative to mpc
modification of the input of solve of the ocpbase (because it takes x_init and x_0, which is already given in x_init)
the OCPCrocoBaseClass, as mentionned earlier
an OCP Param Base class, that can "generalize" the input of the OCP (ie number of iterations of the solver, qp iters if SQP, error, termination condition ...) after the parsing.
a draft on the testing, because i have several issues, especially with the data classes not taking none as input

What I'd like to discuss

OCP Param Base class, I believe that's a valid option as output to the parser (be it from ros, yaml or whatever)

for more information, see https://pre-commit.ci

agimus_controller/agimus_controller/mpc_data.py

agimus_controller/agimus_controller/ocp_base.py

agimus_controller/agimus_controller/ocp_base_croco.py

agimus_controller/agimus_controller/ocp_param_base.py

agimus_controller/tests/test_ocp_base.py

MaximilienNaveau · 2024-12-12T07:56:05Z

agimus_controller/agimus_controller/ocp_param_base.py

+    WeightedTrajectoryPoints: List[
+        WeightedTrajectoryPoint
+    ] | None = None  # List of weighted trajectory points
+    armature: npt.NDArray[np.float64] | None = None  # Armature of the robot


this should go with a RobotModel object that goes into the robot_model class.
We should use the srdf of the robot for this maybe?

srdf would be a hassle, but we could discuss it in a separate PR. For now, i simply put the armature as a property.

agimus_controller/agimus_controller/ocp_param_base.py

agimus_controller/agimus_controller/trajectory.py

MaximilienNaveau · 2024-12-12T07:59:42Z

agimus_controller/tests/test_ocp_croco_base.py

+
+    def test_horizon_size(self):
+        """Test the horizon_size property of the OCPCrocoBase class."""
+        ocp = OCPCrocoBase(self.rmodel, self.cmodel, self.OCPParams)


if you expect a type error to be flag in the test above why this would work?

Kotochleb

Generally, you should avoid introduction of new public function to the abstract class. It was meant to be somewhat fixed so that no one is confused, and we do not end up with compatibility issues like those between for example, default Crocoddyl objects and MiM Solvers.

Additionally your implementation of OCPCrocoBase is not generic at all. It is very specific and you should introduce another class like OCPCrocoReaching where you can impose use of CSQP and your costs. OCPCrocoBase should only implement basic interaction with Crocoddyl and expose maybe new private abstract functions that allow user to choose which solver they want to use and how they want to set up shooting problem object.

Additionally please add docstrigns to OCPBase as you are the first person to extend it, so we will not have to document it later on.

agimus_controller/agimus_controller/ocp_base.py

agimus_controller/agimus_controller/ocp_base_croco.py

Kotochleb · 2024-12-12T12:11:36Z

agimus_controller/agimus_controller/ocp_base_croco.py

+        self,
+        rmodel: pin.Model,
+        cmodel: pin.GeometryModel,
+        OCPParams: OCPParamsCrocoBase,


IMO OCPParamsCrocoBase should be simply arguments of the __init__ function. In C++ it's required due to registers and stuff, but python doesn't care

i believe that having a data class is better for readability / organisation purpose, and so that you don't have gazillion of stuff in the init of the class

But most of them can be passed as defaults. And this is a common pattern in python libraries. Just look at how many parameters matplotlib function have

agimus_controller/agimus_controller/ocp_base_croco.py

agimus_controller/agimus_controller/ocp_param_base.py

agimus_controller/agimus_controller/trajectory.py

Kotochleb · 2024-12-12T12:31:14Z

agimus_controller/tests/test_ocp_param_base.py

+<<<<<<< Updated upstream
+from agimus_controller.agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.agimus_controller.trajectory import (
+    WeightedTrajectoryPoint,
+    TrajectoryPoint,
+    TrajectoryPointWeights,
+)
+
+=======
+from agimus_controller.ocp_param_base import OCPParamsCrocoBase
+from agimus_controller.trajectory import WeightedTrajectoryPoint, TrajectoryPoint, TrajectoryPointWeights
+>>>>>>> Stashed changes


@ArthurH91... Why did you commit merge conflict? Can you do pre-commit install?

well you know sometimes it happens

I do not accept in this case answer that you are just a chill guy

Kotochleb · 2024-12-12T12:32:23Z

agimus_controller/tests/test_ocp_param_base.py

+        dt = np.float64(0.01)
+        T = 100
+        solver_iters = 50


You don't have to cast those and you can pass them in the OCPParamsCrocoBase directly

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

agimus_controller/agimus_controller/ocp_base.py

agimus_controller/agimus_controller/ocp_base_croco.py

MedericFourmy · 2024-12-12T12:43:55Z

agimus_controller/agimus_controller/ocp_base_croco.py

+    def x_init(self) -> np.ndarray:
+        """Initial guess of the states."""
+        x_init = []
+        for TrajectoryPoint in self._OCPParams.WeightedTrajectoryPoints:


snake_case: "TrajectoryPoint" -> "trajectory_point" or "traj_point" or "t_point" etc.

little preference for "trajectory_point"

MedericFourmy · 2024-12-12T13:00:42Z

agimus_controller/agimus_controller/ocp_base_croco.py

+                terminalCostModel.addCost(
+                    "stateReg",
+                    xRegCost,
+                    np.concatenate(


I don't think that the CostModelSum::addCost API take a vector of weights as its third input:
https://github.com/loco-3d/crocoddyl/blob/da92f67394c07c987458a8cb24bc0e33edd64227/include/crocoddyl/core/costs/cost-sum.hxx#L29

For weights regularization, you can define a CostModelResidual with an ActivationModelWeightedQuad, see here or here.

thank you! I was doing it blindly here tbh

MedericFourmy

My main concern is the function solve: maybe creating croco objects everytime would be too costly.

MedericFourmy · 2024-12-12T14:11:06Z

agimus_controller/agimus_controller/ocp_base_croco.py

+        ### Creation of the state and actuation models
+
+        # Stat and actuation model
+        self._state = crocoddyl.StateMultibody(self._rmodel)


In general, I wonder if this is reasonable (in terms of time) to create croco objects (running/terminal costs, Action models etc.) from scratch every time we solve? It's simpler than going through through the croco ocp structure to only but might be a bit slow.
I just checked on a personal codebase a similar function and got:
T = 20 -> 0.7 ms
T = 40 -> 1.5 ms
T = 60 -> 2.7 ms
T = 80 -> 3.8 ms
T = 100 -> 4.5 ms

So it seems that the _state/_actuation creation is negligeable (I'd still put it in __init__ or a setup function) compared to the running cost creation.

ArthurH91 · 2024-12-12T16:08:46Z

My main concern is the function solve: maybe creating croco objects everytime would be too costly.

i totally agree with this, thank you all for the reviews, i'm gonna start working on it tmrw !

agimus_controller/agimus_controller/ocp_base_croco.py

agimus_controller/agimus_controller/ocp_param_base.py

…to x_warmstart for clarity's sake

for more information, see https://pre-commit.ci

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

for more information, see https://pre-commit.ci

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

for more information, see https://pre-commit.ci

Co-authored-by: Naveau <[email protected]>

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

for more information, see https://pre-commit.ci

…ature/create-ocp-param-class

ArthurH91 added 10 commits December 11, 2024 14:09

creating the ocp param base class & the ocp base croco

2b74b10

populate ocp param & croco

60648fc

setting up the params base class

3d36d16

add test for TestOcpBase

0820ea7

change all relative paths to absolute ones

16fbafe

unit tests of the data class ocp param croco base

6ff4507

first try on testing the ocp croco base class

1da7779

ocp croco set up

0105d0c

getting rid of target in the dataclass

4ddeea4

getting rid of target in the dataclass 2

b2eac48

ArthurH91 requested review from petrikvladimir, MaximilienNaveau, MedericFourmy, kzorina, Kotochleb and TheoMF December 11, 2024 17:54

[pre-commit.ci] auto fixes from pre-commit.com hooks

4f07f49

for more information, see https://pre-commit.ci

MaximilienNaveau requested changes Dec 12, 2024

View reviewed changes

ArthurH91 added 3 commits December 12, 2024 11:39

fixing imports

61beafa

fixing imports 2

5d0ced3

removing x0 property ocp base

6be9c9f

Kotochleb suggested changes Dec 12, 2024

View reviewed changes

ArthurH91 and others added 2 commits December 12, 2024 13:58

Update agimus_controller/agimus_controller/trajectory.py

bf1dde7

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

Update agimus_controller/agimus_controller/ocp_base.py

4c42f41

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

MedericFourmy reviewed Dec 12, 2024

View reviewed changes

MedericFourmy requested changes Dec 12, 2024

View reviewed changes

TheoMF requested changes Dec 13, 2024

View reviewed changes

ArthurH91 and others added 2 commits December 13, 2024 11:18

docstring + bringing back x0 in the solve + changing the name x_init …

eab8f12

…to x_warmstart for clarity's sake

[pre-commit.ci] auto fixes from pre-commit.com hooks

894fce4

for more information, see https://pre-commit.ci

ArthurH91 and others added 30 commits January 8, 2025 16:39

recursivity problems fixed

3ba81d4

Update agimus_controller/agimus_controller/factory/robot_model.py

57b2f36

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

[pre-commit.ci] auto fixes from pre-commit.com hooks

ce26d70

for more information, see https://pre-commit.ci

changing the order of the methods

b2afd0f

Update agimus_controller/agimus_controller/factory/robot_model.py

1c8a1e3

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

[pre-commit.ci] auto fixes from pre-commit.com hooks

70a73ee

for more information, see https://pre-commit.ci

add the color as parameter

393ff30

correcting typo in type for urdf

406a689

unit testing the dataclass

12d17ef

unit testing the dataclass2

80062e6

wrapping up unit testing for rmf

720cf23

getting rid of explicit casting

f5fed68

Update agimus_controller/agimus_controller/factory/robot_model.py

ea73d2e

Co-authored-by: Naveau <[email protected]>

typo

620f4b0

free flyer exception in the unit test of armature

2c55b75

bring back path objects

a91e554

integration test to check that model is working well with pinocchio

294f5b9

merging robot refactor

4a7cedd

fixing unit test ocp croco base param

d805ea4

Update agimus_controller/tests/test_robot_models.py

d53b0f6

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

Update agimus_controller/agimus_controller/factory/robot_model.py

086bcca

Co-authored-by: Krzysztof Wojciechowski <[email protected]>

[pre-commit.ci] auto fixes from pre-commit.com hooks

3e59282

for more information, see https://pre-commit.ci

getting rid of str in paths + collision color in param

ca1066b

fixing the tests

488ab70

changing locked joint into var

b3c121b

self collision test improved

a3ee5c5

test reduced model

6fcf107

Merge branch 'ahaffemayer/refactor-robot-factory' into ahaffemayer/fe…

3ddf226

…ature/create-ocp-param-class

changing factory to models

d6caca3

first two unit tests working

faf187e

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

OCP Croco Class #112

OCP Croco Class #112

ArthurH91 commented Dec 11, 2024 •

edited

Loading

MaximilienNaveau Dec 12, 2024

ArthurH91 Dec 13, 2024

MaximilienNaveau Dec 12, 2024

Kotochleb left a comment

Kotochleb Dec 12, 2024

ArthurH91 Dec 13, 2024

Kotochleb Dec 13, 2024

Kotochleb Dec 12, 2024

ArthurH91 Dec 12, 2024

Kotochleb Dec 12, 2024 •

edited

Loading

Kotochleb Dec 12, 2024

MedericFourmy Dec 12, 2024

MaximilienNaveau Dec 12, 2024

MedericFourmy Dec 12, 2024

ArthurH91 Jan 2, 2025

MedericFourmy left a comment

MedericFourmy Dec 12, 2024

ArthurH91 commented Dec 12, 2024

OCP Croco Class #112

Are you sure you want to change the base?

OCP Croco Class #112

Conversation

ArthurH91 commented Dec 11, 2024 • edited Loading

Intro

What's in the PR

What I'd like to discuss

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Kotochleb left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Kotochleb Dec 12, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MedericFourmy left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ArthurH91 commented Dec 12, 2024

ArthurH91 commented Dec 11, 2024 •

edited

Loading

Kotochleb Dec 12, 2024 •

edited

Loading