Feature/improve wrench computation #1343
Open
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This comment was marked as resolved.
This comment was marked as resolved.
rjgriffin42
force-pushed
the
feature/improve-wrench-computation
branch
from
ed9ff75 to
80845bd
Compare
nim65s
approved these changes
Feb 11, 2025
cmastalli
requested changes
Feb 11, 2025
| @@ -6,6 +6,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). | |||
|
|
|||
| ## [Unreleased] | |||
|
|
|||
| * Updated the rotation transpose in contact wrench cone to re-use the same object to avoid additional transpose operations. | |||
There was a problem hiding this comment.
Please indicate the PR as in the examples above. Also, notice that there is a conflict with other changes in the changelog.
| * | ||
| * @param[in] cost_item Cost item | ||
| */ | ||
| void addCost(const boost::shared_ptr<CostItem>& cost_item); |
There was a problem hiding this comment.
- We recently moved to std::shared_ptr, as you can see in this {boost -> std}::shared_ptr #1339.
- I suggest defining
weight(in the code above) as a default argument. This will require to update the Python bindings.
Suggested change
| void addCost(const boost::shared_ptr<CostItem>& cost_item); |
There was a problem hiding this comment.
The default value should be 1, of course.
Comment on lines
+54
to
+73
| template <typename Scalar> | ||
| void CostModelSumTpl<Scalar>::addCost(const boost::shared_ptr<CostItem>& cost_item) | ||
| { | ||
| if (cost_item->cost->get_nu() != nu_) { | ||
| throw_pretty( | ||
| cost_item->name | ||
| << " cost item doesn't have the same control dimension (it should be " + | ||
| std::to_string(nu_) + ")"); | ||
| } | ||
| costs_.insert(std::make_pair(cost_item->name, cost_item)); | ||
| if (cost_item->active) { | ||
| nr_ += cost_item->cost->get_activation()->get_nr(); | ||
| nr_total_ += cost_item->cost->get_activation()->get_nr(); | ||
| active_set_.insert(cost_item->name); | ||
| } else { | ||
| nr_total_ += cost_item->cost->get_activation()->get_nr(); | ||
| inactive_set_.insert(cost_item->name); | ||
| } | ||
| } | ||
|
|
There was a problem hiding this comment.
From the previous comment, we need to remove this
Suggested change
| template <typename Scalar> | |
| void CostModelSumTpl<Scalar>::addCost(const boost::shared_ptr<CostItem>& cost_item) | |
| { | |
| if (cost_item->cost->get_nu() != nu_) { | |
| throw_pretty( | |
| cost_item->name | |
| << " cost item doesn't have the same control dimension (it should be " + | |
| std::to_string(nu_) + ")"); | |
| } | |
| costs_.insert(std::make_pair(cost_item->name, cost_item)); | |
| if (cost_item->active) { | |
| nr_ += cost_item->cost->get_activation()->get_nr(); | |
| nr_total_ += cost_item->cost->get_activation()->get_nr(); | |
| active_set_.insert(cost_item->name); | |
| } else { | |
| nr_total_ += cost_item->cost->get_activation()->get_nr(); | |
| inactive_set_.insert(cost_item->name); | |
| } | |
| } |
Comment on lines
-159
to
+162
| A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_.transpose(); | ||
| A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_transpose; |
There was a problem hiding this comment.
I have made these improvements in private, but I can discard them.
Suggested change
| A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_.transpose(); | |
| A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_transpose; | |
| A_.row(2 * i).template head<3>() = (mu_nsurf + tsurf_i).transpose() * R_transpose; |
| @@ -299,6 +302,7 @@ void WrenchConeTpl<Scalar>::set_box(const Vector2s& box) { | |||
|
|
|||
| template <typename Scalar> | |||
| void WrenchConeTpl<Scalar>::set_mu(const Scalar mu) { | |||
| mu_ = mu; | |||
There was a problem hiding this comment.
Nice catch! Thanks for fixing this.
This also means we're not properly unit-testing this. I would define a test_setter function in https://github.com/loco-3d/crocoddyl/blob/master/unittest/test_wrench_cone.cpp
| } | ||
| A_.row(nf_).head(3) = R_.col(2).transpose(); | ||
| A_.row(nf_).head(3) = R_transpose.row(2); |
There was a problem hiding this comment.
Suggested change
| A_.row(nf_).head(3) = R_transpose.row(2); | |
| A_.row(nf_).template head<3>() = R_transpose.row(2); |
| @@ -156,26 +159,26 @@ void WrenchConeTpl<Scalar>::update() { | |||
| Scalar theta_i = theta * static_cast<Scalar>(i); | |||
| Vector3s tsurf_i = Vector3s(cos(theta_i), sin(theta_i), Scalar(0.)); | |||
| Vector3s mu_nsurf = -mu * Vector3s::UnitZ(); | |||
| A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_.transpose(); | |||
| A_.row(2 * i).head(3) = (mu_nsurf + tsurf_i).transpose() * R_transpose; | |||
| A_.row(2 * i + 1).head(3) = | |||
There was a problem hiding this comment.
Suggested change
| A_.row(2 * i + 1).head(3) = | |
| A_.row(2 * i + 1).template head<3>() = |
| } | ||
|
|
||
| // Create a temporary object for computation | ||
| Matrix3s R_transpose = R_.transpose(); |
There was a problem hiding this comment.
Should we allocate this data once (i.e., as a member of the structure)? Not sure if it is justified in this case. Any thought?
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
No description provided.