-
Notifications
You must be signed in to change notification settings - Fork 36
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.
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
add Impedance task #100
add Impedance task #100
Conversation
32dc60b
to
3f279ee
Compare
I add two notes about the force control of mc_rtc. 1. Relationship between ImpedanceTask and AdmittanceTaskAs shown in the attached PDF, the current AdmittanceTask in mc_rtc (and the force control presented in this paper) are a special case of impedance control. I wrote a small document about this, see AdmittanceTask_mc_rtc_20201206.pdf. 2. Class hierarchy of force control tasksBecause AdmittanceTask, DampingTask, and ComplianceTask are all special cases of ImpedanceTask, I suggest the following better class hierarchy. (Furthermore, AdmittanceTask and DampingTask do almost the same thing, so I don't think they need to be separated.) These notes are almost same as suggested in the internal MergeRequest linked in the first comment. |
db9bf27
to
125aa1b
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks @mmurooka !
Overall this looks good, just a few minor comments and suggestions. You can commit most of them directly through the github interface ("Add suggestion to batch", and then "Commit Suggestion").
I just have a minor concern about the inconsistent units of impM/impD/impK (kg in the constructor, newtons in the setters).
Then let's try this on the robot ;)
src/mc_tasks/ImpedanceTask.cpp
Outdated
// 2.1 Integrate velocity to pose | ||
sva::PTransformd T_0_c(compPoseW_.rotation()); | ||
// Represent the compliance velocity and acceleration in the compliance frame and scale by dt | ||
sva::MotionVecd compDeltaC = T_0_c * (dt * compVelW_ + 0.5 * dt * dt * compAccelW_); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This can be factorized:
sva::MotionVecd compDeltaC = T_0_c * (dt * compVelW_ + 0.5 * dt * dt * compAccelW_); | |
sva::MotionVecd compDeltaC = T_0_c * dt * (compVelW_ + 0.5 * dt * compAccelW_); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
src/mc_tasks/ImpedanceTask.cpp
Outdated
// Convert the angular velocity to the rotation matrix through AngleAxis representation | ||
Eigen::AngleAxisd aaCompDeltaC(Eigen::Quaterniond::Identity()); | ||
if(compDeltaC.angular().norm() > 1e-6) | ||
{ | ||
aaCompDeltaC = Eigen::AngleAxisd(compDeltaC.angular().norm(), compDeltaC.angular().normalized()); | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think it would be nice to have a more direct way to do this kind of integration in SpaceVecAlg
. I'm not sure how to best implement it though (ping @mehdi-benallegue)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thank you for your review!
rootLinkTask = | ||
std::make_shared<mc_tasks::EndEffectorTask>("base_link", robots(), robots().robotIndex(), 100.0, 10000.0); | ||
rootLinkTask->set_ef_pose(sva::PTransformd(Eigen::Vector3d(0, 0, 1))); | ||
solver().addTask(rootLinkTask); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why a task on the root link?
Not so much reason. Just to make a example with most simple settings, the arm of JVRC1 is considered as a manipulator fixed to the ground.
Instead of it, adding foot/floor contact, dynamicsConstraint, CoM task is also good for humanoid example.
src/mc_tasks/ImpedanceTask.cpp
Outdated
// Set impedance parameters | ||
impedance(sva::ForceVecd(Eigen::Vector3d::Constant(10.0 * impM), Eigen::Vector3d::Constant(impM)), | ||
sva::ForceVecd(Eigen::Vector3d::Constant(10.0 * impD), Eigen::Vector3d::Constant(impD)), | ||
sva::ForceVecd(Eigen::Vector3d::Constant(10.0 * impK), Eigen::Vector3d::Constant(impK))); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
impM/impD/impK
are sva::ForceVecd
instance and the unit is different for the force part and moment part. For example, the unit of impM
's force part is kg
, and the unit of impM
's moment part is kg m^2
(same with moment of inertia). Therefore, the impedance parameters of the force and moment components cannot be compared.
Ten times comes from the experience of being too sensitive to moments when the same impedance parameters are applied to both forces and moments. Multiplying all of MDK by 10 makes it less insensitive to external moment.
Thanks to the advice from @mitsuharu-morisawa, I updated the impedance equation implementation a little bit after the first review: 40265e6 . There is no change in behavior before and after this commit if deisred pose, velocity, and acceleration are consistent. If desired pose, velocity, and acceleration are NOT consistent (e.g., desired pose is updated with ZERO desired velocity and acceleration), the new implementation, which deals only with the amount of modification ( |
4608f50
to
ac2574b
Compare
I added the JSON schema, we can now add and test the task directly from the GUI. Note that we will need to do a few code changes later on, as the YAML properties inherited from |
Thanks! I could add a new ImpedanceTask from Rviz panel. It seems that we don't need any code changes ;) |
0df36be
to
62fa380
Compare
CI seems to start to fail with some issue unrelated to PR.. https://github.com/jrl-umi3218/mc_rtc/pull/100/checks?check_run_id=1554274341#step:5:1556 /package/mc_rtc/src/mc_rbdyn/Robot.cpp:1236:5: error: 'Quaterniond' is not a member of 'sva'
sva::Quaterniond rotation{pt.rotation().transpose()};
^
/package/mc_rtc/src/mc_rbdyn/Robot.cpp:1236:5: note: suggested alternative:
In file included from /usr/include/eigen3/Eigen/Geometry:42:0,
from /usr/include/SpaceVecAlg/SpaceVecAlg:13,
from /usr/include/RBDyn/Joint.h:16,
from /package/mc_rtc/include/mc_rbdyn/Base.h:9,
from /package/mc_rtc/include/mc_rbdyn/Robot.h:7,
from /package/mc_rtc/src/mc_rbdyn/Robot.cpp:5:
/usr/include/eigen3/Eigen/src/Geometry/Quaternion.h:303:28: note: 'Eigen::Quaterniond'
typedef Quaternion<double> Quaterniond;
^ |
@mmurooka Yes. This is related to jrl-umi3218/SpaceVecAlg#37 and fixed by a13a66f so if you rebase on master it should be fine |
fb02c40
to
79c2787
Compare
Pierre was faster than me there, I've rebased it onto master and will now look into the last steps to finalize this PR :) |
1fee7f0
to
4ffdfe3
Compare
This is almost finalized:
TODO:
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thank you for finalizing!
src/mc_tasks/ImpedanceTask.cpp
Outdated
|
||
if(config.has("target")) | ||
{ | ||
desiredPose(config("target")); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Maybe we should remove this part to avoid the conflict with
https://github.com/jrl-umi3218/mc_rtc/pull/100/files#diff-1ea7af2780604d3059658bfd0c8f69e98c1a603be1810ec9c1828df358f272a1R205
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nice catch, done in 9558f32
Hop all done, and extensively tested on HRP5 (with both the bobbin, and the stabilizer in #95). I think this can be merged when CI passes. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Looks good but I would like to clean up the API a bit before we merge this
include/mc_tasks/ImpedanceTask.h
Outdated
const Eigen::Vector3d wrenchGainLinUpper_ = Eigen::Vector3d::Constant(1e6); | ||
const Eigen::Vector3d wrenchGainLinLower_ = Eigen::Vector3d::Constant(0); | ||
const Eigen::Vector3d wrenchGainAngUpper_ = Eigen::Vector3d::Constant(1e6); | ||
const Eigen::Vector3d wrenchGainAngLower_ = Eigen::Vector3d::Constant(0); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
These members could be static
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Also, while I agree we want to enforce that all gains are (strictly) positive, maybe the upper limit does not make a lot of sense? Furthermore, if they are all the same, I think having separate limits is a bit overkill.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yes, I think the upper bounds may be omitted for all variables.
As for lower bounds, it should be strictly positive for M
, and zero or positive for other variables, but I think separate limits are unnecessary.
{ | ||
mc_rtc::log::error_and_throw<std::runtime_error>( | ||
"The \"Impedance\" controller requires a robot with a fixed base (base of robot {} has {} DoF)", robot().name(), | ||
robot().mb().joint(0).dof()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think this sample could be written to support both fixed and non-fixed base robots, similar to the EndEffector sample.
include/mc_tasks/ImpedanceTask.h
Outdated
double impM = 10, | ||
double impD = 1000, | ||
double impK = 1000, | ||
double oriScale = 1); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't think we want to have the impedance parameters in the constructor (we don't have them in AdmittanceTask
and DampingTask
for example).
include/mc_tasks/ImpedanceTask.h
Outdated
sva::ForceVecd impM_; // must be set in the Constructor | ||
sva::ForceVecd impD_; // must be set in the Constructor | ||
sva::ForceVecd impK_; // must be set in the Constructor | ||
sva::MotionVecd wrenchGain_ = sva::MotionVecd::Zero(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
sva::ImpedanceVecd
is a better match for this kind of parameters, it's similar to sva::ForceVecd
but has linear()
and angular()
accessors which are more explicit. Although they are not super well supported by mc_rtc so I will make a PR for this later today.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Although they are not super well supported by mc_rtc so I will make a PR for this later today.
This is done in #112
include/mc_tasks/ImpedanceTask.h
Outdated
} | ||
|
||
/*! \brief Get the desired pose of the surface in the world frame. */ | ||
const sva::PTransformd & desiredPose() const noexcept |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We use target
rather than desired
everywhere else so for coherency it would be better to use target
here as well (this comment applies to every use of desired
in this class and the associated schema)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Originally, I intentionally used a different term than target. The desiredPose specified by the user is modified by the impedance equation, and it is passed to the target of the SurfaceTransformTask: https://github.com/mmurooka/mc_rtc/blob/519a88a24b65b0bd7472dfca4b0833aaba1d74c3/src/mc_tasks/ImpedanceTask.cpp#L141
I thought that overriding the target method in ImpedanceTask would cause the following two return values to be different, which would be confusing:
std::shared_ptr<ImpedanceTask> task;
task->target();
std::dynamic_pointer_cast<SurfaceTransformTask>(task)->target();
However, it doesn't matter in normal usage, and target may be easier for the user to understand, so either one is fine.
mc_filter::utils::clampAndWarn(gain.linear(), wrenchGainLinLower_, wrenchGainLinUpper_, "wrenchGainLinear"); | ||
wrenchGain_.angular() = | ||
mc_filter::utils::clampAndWarn(gain.angular(), wrenchGainAngLower_, wrenchGainAngUpper_, "wrenchGainAngular"); | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
How about adding an mc_tasks::force::ImpedanceGains
class to lighten the mc_tasks::force::AdmittanceTask
API?
Something like (simplified for the review):
struct ImpedanceGains
{
/* relevant accessors */
protected:
// Either sva::ImpedanceVecd or an internal wrapper that ensures the gains are (strictly) positive
sva::ImpedanceVecd M_;
sva::ImpedanceVecd D_;
sva::ImpedanceVecd K_;
sva::ImpedanceVecd wrench_;
};
mc_tasks::force::AdmittanceTask task;
// For example, instead of:
task.impedanceK(...);
// We get
task.gains().K(...);
// And with a nice wrapper for ensuring the provided gains are positive we could have:
task.gains().K().linear(...);
// Without having to add as many functions in ImpedanceTask itself
Down the line, if we factorize the other force tasks as suggested we can specialized the ImpedanceGains
accordingly and expose a specific gains()
method for each force task rather than painstakingly choosing and picking specific methods
src/mc_tasks/ImpedanceTask.cpp
Outdated
mc_rtc::gui::Transform("pose", [this]() { return this->surfacePose(); }), | ||
// impedance parameters | ||
mc_rtc::gui::ArrayInput("impedanceM", {"cx", "cy", "cz", "fx", "fy", "fz"}, | ||
[this]() { return this->impedanceM().vector(); }, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
FYI, you can return a const sva::ForceVecd &
here as the GUI serializer knows how to transform it into an array
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
And with #112 it's actually a little better to do so as this incurs no additional copy to get the vector
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
FYI, you can return a const sva::ForceVecd & here as the GUI serializer knows how to transform it into an array
Although ForceVecd::vector()
does not return const reference, can we return const reference in GUI?
https://github.com/jrl-umi3218/SpaceVecAlg/blob/1668efd3f2647c6a850d268b73ce21930ebbeb38/src/SpaceVecAlg/ForceVec.h#L88
@gergondet , Thank you for review! |
…robust to the desired position error.
Co-authored-by: TANGUY Arnaud <[email protected]>
42ebb40
to
3037f48
Compare
@mmurooka thanks for your answer, I've pushed what I had in mind for I went ahead with a few other changes discussed in the PR. Could you test the changes in a simulation? I don't think I have the corresponding JVRC1 environment for testing the sample. |
Thank you for update. I tried Impedance controller in choreonoid but, it seems something has broken... During simulation, the following warnings continue to be output:
I thought that it is just doing
with the following mc_rtc.yaml
Is there anything missing to reproduce this in your side? |
5442a57
to
406a901
Compare
@mmurooka Indeed I have everything to test it here I have now fixed the sample behavior for both the fixed base and floating base variant of JVRC1 For reference, I had mistakenly inverted the angular/linear gains And more importantly, It should be ok now |
@gergondet Thank you. I now checked a simulation works correctly for fixed and non-fixed JVRC1. I read through the changes in the source code and it's OK. I changed the damping parameter of sample controller because the impedance vibrates and diverges when adding large force interactively by clicking and dragging in Choreonoid (also for the real robot, the critical damping value does not work well for ImpedanceTask). Only one concern is that the default impedance gain can be dangerous in some cases. If we change the wrench gain to 1 (it is zero for default) with keeping MDK with initial value (M=1e-6 and D,K=0), it diverges. It could easily happen by user's operation for example when adding task interactively like https://www.dropbox.com/s/wh6z3ijpmcggp9n/impedance-task-from-rviz-20201210.mp4?dl=0 (this link is copied from #100 (comment)) |
1261d76
to
712587c
Compare
Thanks @mmurooka , that's a great point. I've changed the default gains in I'm also thinking of changing the GUI of the gains so that they can be set in a bulk rather than having to change each parameters |
I added small two commits (Just in case it is not erased by force push).
Thank you, I confirmed it worked as intended.
It will be useful (although we also need to keep the element-wise interface). |
Quick note that just came to me: forms do not (yet) support having their default data fed from a callbacks so I will actually revert that change since the GUI would not reflect changes made programmatically to the gains |
…a single form" This reverts commit a396e59. The reason is that currently we cannot set the default form values from a callback and thus the form would not reflect changes made programmatically.
OK, then I think we can leave it as it is. (And also I thought the order of elements (i.e., angular followed by linear) is not clear for beginners in the form style). |
Add the task to move the end-effector with impedance dynamics. Unlike AdmittanceTask or DampingTask, a robot can adapt to the external force while following the target position. For example, ImpedanceTask is necessary when pushing an object with a constant force (both force and position targets need to be considered).
This impedance task corresponds to what @stephane-caron calls "impedance control" below: https://gite.lirmm.fr/multi-contact/mc_rtc/-/merge_requests/133#note_9535 (this link is internal only)
The movie of the sample controller (MCImpedanceController) added in this PR: https://www.dropbox.com/s/cjkdb9u5ihyotnz/impedance-controller-20201205.mp4?dl=0