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.

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

Merged
merged 42 commits into from
Jan 8, 2021
Merged

add Impedance task #100

merged 42 commits into from
Jan 8, 2021

Conversation

mmurooka
Copy link
Member

@mmurooka mmurooka commented Dec 5, 2020

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

@mmurooka
Copy link
Member Author

mmurooka commented Dec 6, 2020

I add two notes about the force control of mc_rtc.

1. Relationship between ImpedanceTask and AdmittanceTask

As 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 tasks

Because 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.)
But this is outside the scope of this Pull Request as it will affect the existing codes.

class-hierarchy

These notes are almost same as suggested in the internal MergeRequest linked in the first comment.

@mmurooka mmurooka force-pushed the impedance-task branch 2 times, most recently from db9bf27 to 125aa1b Compare December 8, 2020 13:31
@arntanguy arntanguy self-assigned this Dec 10, 2020
Copy link
Collaborator

@arntanguy arntanguy left a 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 ;)

include/mc_tasks/ImpedanceTask.h Outdated Show resolved Hide resolved
include/mc_tasks/ImpedanceTask.h Outdated Show resolved Hide resolved
include/mc_tasks/ImpedanceTask.h Outdated Show resolved Hide resolved
include/mc_tasks/ImpedanceTask.h Outdated Show resolved Hide resolved
include/mc_tasks/ImpedanceTask.h Outdated Show resolved Hide resolved
// 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_);
Copy link
Collaborator

Choose a reason for hiding this comment

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

This can be factorized:

Suggested change
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_);

Copy link
Member Author

Choose a reason for hiding this comment

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

updated by ac2574b and 3d59973 .

Comment on lines 69 to 86
// 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());
}
Copy link
Collaborator

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)

src/mc_tasks/ImpedanceTask.cpp Outdated Show resolved Hide resolved
src/mc_tasks/ImpedanceTask.cpp Show resolved Hide resolved
src/mc_tasks/ImpedanceTask.cpp Outdated Show resolved Hide resolved
Copy link
Member Author

@mmurooka mmurooka left a 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);
Copy link
Member Author

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.

// 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)));
Copy link
Member Author

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.

src/mc_tasks/ImpedanceTask.cpp Show resolved Hide resolved
@mmurooka
Copy link
Member Author

mmurooka commented Dec 10, 2020

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 (deltaComp****W_) in the impedance equation, more robustly follows the desired pose.

@arntanguy
Copy link
Collaborator

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 SurfaceTransformTask are not all valid as-is (they call target() instead of your desiredPose(...) for instance).

@mmurooka
Copy link
Member Author

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

Thanks! I could add a new ImpedanceTask from Rviz panel. It seems that we don't need any code changes ;)
https://www.dropbox.com/s/wh6z3ijpmcggp9n/impedance-task-from-rviz-20201210.mp4?dl=0

@mmurooka
Copy link
Member Author

mmurooka commented Dec 11, 2020

  • clamp gains (especially non-zero for impedance M to avoid zero devision). (done 71fa092)

  • clamp the compliance delta pose. (done 29db680)

  • apply low pass filter to the measured wrench. (done 9d501ee)

  • set the initial target from yaml.

  • enable to set consistent impedance MDK. (optional)

@mmurooka
Copy link
Member Author

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;
                            ^

@gergondet
Copy link
Member

CI seems to start to fail with some issue unrelated to PR..

#100 (checks)

@mmurooka Yes. This is related to jrl-umi3218/SpaceVecAlg#37 and fixed by a13a66f so if you rebase on master it should be fine

@arntanguy
Copy link
Collaborator

Pierre was faster than me there, I've rebased it onto master and will now look into the last steps to finalize this PR :)

@arntanguy
Copy link
Collaborator

arntanguy commented Dec 21, 2020

This is almost finalized:

  • 5466479 adds a fixed-based variant of the JVRC1 robot module named JVRC1Fixed
  • 97ea60e improves the sample controller to use the fixed-based robot, and make the trajectory start smoothly at the current position
  • 5c375f4 Fixes a numerical issue with the BodySensor observer that only occurs with fixed-based robots: when repeatedly updating the pose of the floating base, the computation of relative transformation between the IMU sensor and the floating base becomes de-orthogonalized and de-normalized. This could be avoided by computing this relative pose without passing though world-frame, I'll introduce this later. For now this reorthogonalizes the transformation. (this doesn't directly affect this PR but affects the mc_force_sensor_calibration controller)
  • 40aee1d correctly loads the task's target from YAML. I went the simplest way of setting the compliance pose the the same as the TrajectoryTaskGeneric's one.
  • 5676705 adds a tutorial page about all the available sample controllers, with relevant links to the corresponding API/YAML/other tutorials/online demos.

TODO:

  • Make the mc_force_sensor_calibration controller public
  • Display a mc_rtc::gui::Trajectory element for the sample controller

Copy link
Member Author

@mmurooka mmurooka left a 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!


if(config.has("target"))
{
desiredPose(config("target"));
Copy link
Member Author

@mmurooka mmurooka Dec 21, 2020

Choose a reason for hiding this comment

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

Copy link
Collaborator

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

@arntanguy
Copy link
Collaborator

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.
@gergondet Any further comments on this PR?

Copy link
Member

@gergondet gergondet left a 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

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);
Copy link
Member

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

Copy link
Member

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.

Copy link
Member Author

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());
Copy link
Member

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.

double impM = 10,
double impD = 1000,
double impK = 1000,
double oriScale = 1);
Copy link
Member

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).

src/mc_robots/jvrc1.h Outdated Show resolved Hide resolved
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();
Copy link
Member

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.

Copy link
Member

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

}

/*! \brief Get the desired pose of the surface in the world frame. */
const sva::PTransformd & desiredPose() const noexcept
Copy link
Member

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)

Copy link
Member Author

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");
}
Copy link
Member

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 Show resolved Hide resolved
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(); },
Copy link
Member

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

Copy link
Member

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

Copy link
Member Author

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

@mmurooka
Copy link
Member Author

mmurooka commented Jan 5, 2021

@gergondet , Thank you for review!
I generally agree with the comments. However, I'm not sure if I can reflect your comments exactly as you intended, so I would appreciate it if you could change some (especially about #100 (comment)) or all directly when you have time.

@gergondet
Copy link
Member

@mmurooka thanks for your answer, I've pushed what I had in mind for ImpedanceGains, you can see it in 3037f48 (#100)

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.

@mmurooka
Copy link
Member Author

mmurooka commented Jan 7, 2021

Thank you for update. I tried Impedance controller in choreonoid but, it seems something has broken...
video: https://www.dropbox.com/s/n3fy3ykakjb4na6/impedance_20210107.mp4?dl=0

During simulation, the following warnings continue to be output:

[warning] angular deltaCompAccel limited from 2788.7463790536262 to 1000.0
[warning] linear deltaCompPose limited from 1.0001272017767997 to 1.0
[warning] linear deltaCompAccel limited from 2457.9901027281644 to 1000.0

I don't think I have the corresponding JVRC1 environment for testing the sample.

I thought that it is just doing

cd ~/openrtp/share/hrpsys/samples/JVRC1 # or corresponding directory
choreonoid sim_mc_fixed.cnoid --start-simulation

with the following mc_rtc.yaml

MainRobot: JVRC1Fixed
Enabled: Impedance

Is there anything missing to reproduce this in your side?

@gergondet
Copy link
Member

@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 4f82a63 (#100)

And more importantly, spring() was not returning the correct gain at all 328c6cd (#100)

It should be ok now

@mmurooka
Copy link
Member Author

mmurooka commented Jan 7, 2021

@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.
We have encountered divergence of ImpedanceTask on real robots many times and I'd like to keep the source code safe side.

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))

@gergondet
Copy link
Member

Thanks @mmurooka , that's a great point. I've changed the default gains in 71b04b9 (#100) so that they match the ones you were setting before.

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

@mmurooka
Copy link
Member Author

mmurooka commented Jan 7, 2021

I added small two commits (Just in case it is not erased by force push).

I've changed the default gains in 71b04b9 (#100) so that they match the ones you were setting before.

Thank you, I confirmed it worked as intended.

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

It will be useful (although we also need to keep the element-wise interface).

@gergondet
Copy link
Member

It will be useful (although we also need to keep the element-wise interface).

This looks like this in the latest change (with a patch to mc_rtc_ros I just published):

image

You can still edit a single gain but they are also all sent (and changed) at the same time

@gergondet
Copy link
Member

You can still edit a single gain but they are also all sent (and changed) at the same time

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.
@mmurooka
Copy link
Member Author

mmurooka commented Jan 8, 2021

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).

@gergondet gergondet merged commit 2177ecf into jrl-umi3218:master Jan 8, 2021
@mmurooka mmurooka deleted the impedance-task branch January 12, 2021 00:16
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants