Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add ocp exp3 #126

Merged
merged 29 commits into from
Jan 23, 2025
Merged

Add ocp exp3 #126

merged 29 commits into from
Jan 23, 2025

Conversation

ArthurH91
Copy link
Collaborator

This is the OCP for exp3.

It hasn't been tested yet but that's the gist of it.

@MedericFourmy
Copy link

I would not use the term "exp3". This OCP is meant to be used for the agimus demo3 . I would use something more meaninfull such as joint_state, so:
ocp_exp3.py -> ocp_croco_joint_state.py
OCPCrocoExp3 -> OCPCrocoJointState

@ArthurH91 ArthurH91 marked this pull request as ready for review January 16, 2025 17:58
MaximilienNaveau
MaximilienNaveau reviewed Jan 16, 2025
View reviewed changes
Copy link
Collaborator

@MaximilienNaveau MaximilienNaveau left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

some comments but it looks good

agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/tests/test_ocp_croco_joint_state.py Outdated Show resolved Hide resolved
Kotochleb
Kotochleb suggested changes Jan 17, 2025
View reviewed changes
agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/tests/test_ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/tests/test_ocp_croco_joint_state.py Outdated Show resolved Hide resolved
agimus_controller/tests/test_ocp_croco_joint_state.py Outdated Show resolved Hide resolved
ArthurH91 and others added 11 commits January 17, 2025 15:45
@ArthurH91 @MaximilienNaveau
Update agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py
a486114 
Co-authored-by: Naveau <[email protected]>
@ArthurH91 @MaximilienNaveau
Update agimus_controller/tests/test_ocp_croco_joint_state.py
4af177b 
Co-authored-by: Naveau <[email protected]>
@ArthurH91 @Kotochleb
Update agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py
096d43d 
Co-authored-by: Krzysztof Wojciechowski <[email protected]>
@ArthurH91 @Kotochleb
Update agimus_controller/agimus_controller/ocp/ocp_croco_joint_state.py
2ce89d1 
Co-authored-by: Krzysztof Wojciechowski <[email protected]>
@ArthurH91 @Kotochleb
Update agimus_controller/tests/test_ocp_croco_joint_state.py
feb7dc5 
Co-authored-by: Krzysztof Wojciechowski <[email protected]>
@ArthurH91 @Kotochleb
Apply suggestions from code review
99bbc0e 
Co-authored-by: Krzysztof Wojciechowski <[email protected]>
@ArthurH91
fixing naming of controle regul + changing values in the ocp
2c8f5b6
@ArthurH91
Merge branch 'topic/humble-devel/refactor' into ahaffemayer/feature/a…
8a7a977 
…dd-ocp-exp3
@ArthurH91
Merge branch 'bugfix/loading-reduced-model' into ahaffemayer/feature/…
17b14a5 
…add-ocp-exp3
@ArthurH91
changing ocp to tracking target because it works
8339410
@ArthurH91
ocp working with test
4ebd3ad
@MedericFourmy
Copy link

MedericFourmy commented Jan 20, 2025 •
edited
Loading

Seems like all comments were addressed, I'm in favor of merging this PR.

Kotochleb
Kotochleb suggested changes Jan 21, 2025
View reviewed changes
agimus_controller/agimus_controller/ocp/ocp_croco_goal_reaching.py Outdated Show resolved Hide resolved
agimus_controller/agimus_controller/ocp/ocp_croco_goal_reaching.py Outdated
Comment on lines 204 to 209
ee_cost.cost.residual.reference = reference_weighted_trajectory[
-1
].point.end_effector_poses["panda_hand_tcp"]
ee_cost.cost.activation.weights = reference_weighted_trajectory[
i
].weight.w_end_effector_poses["panda_hand_tcp"]
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you think panda_hand_tcp should be hardcoded?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes because if you don't want to use this ocp you just recreate one

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would still have it as a parameter, maybe set in init, default to panda_hand_tcp (and maybe a check that robot model actually has this one)

Copy link

@MedericFourmy MedericFourmy Jan 22, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we should avoid hardcoding things such as weights or names as much as it can be. This ocp implementation respects this principle quite well already, except for panda_hand_tcp.

ResidualModelFramePlacement() does require a frame_id as an input to its constructor function but
I think I would put a dummy value, like the rest of the weights/references:

            framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
                self._state,
                0,
                pin.SE3.Identity(),
            )

and then update it in set_reference_weighted_trajectory using sth like:

# setting running model goal tracking reference, weight and frame id
# assuming exactly one end-effector tracking reference was passed to the trajectory
ee_name = reference_weighted_trajectory[i].weight.w_end_effector_poses.keys()[0]  
ee_id = self._robot_models.robot_model.getFrameId("panda_hand_tcp")
ee_cost = self._solver.problem.runningModels[i].differential.costs.costs["goalTracking"]
ee_cost.set_id(ee_id)
ee_cost.cost.activation.weights = reference_weighted_trajectory[i].weight.w_end_effector_poses[ee_name]
ee_cost.cost.residual.reference = reference_weighted_trajectory[i].point.end_effector_poses[ee_name]

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fair enough, i'm committing the changes

agimus_controller/agimus_controller/ocp/ocp_croco_goal_reaching.py Outdated Show resolved Hide resolved
@MaximilienNaveau MaximilienNaveau linked an issue Jan 21, 2025 that may be closed by this pull request
[MPC] prepare OCP to test demo3 #142
Open
@ArthurH91
Copy link
Collaborator Author

Done. Should be ready to merge now.

@MaximilienNaveau MaximilienNaveau merged commit a8dac7c into agimus-project:topic/humble-devel/refactor Jan 23, 2025
2 checks passed
@MaximilienNaveau
Copy link
Collaborator

Thanks for the work!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@TheoMF TheoMF TheoMF requested changes

@kzorina kzorina kzorina left review comments

@MedericFourmy MedericFourmy MedericFourmy approved these changes

@MaximilienNaveau MaximilienNaveau MaximilienNaveau left review comments

@jmirabel jmirabel jmirabel left review comments

@Kotochleb Kotochleb Kotochleb requested changes

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

[MPC] prepare OCP to test demo3
7 participants
@ArthurH91 @MedericFourmy @MaximilienNaveau @jmirabel @kzorina @Kotochleb @TheoMF