Skip to content
New issue

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

#

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

Already on GitHub? # to your account

[StabilizerTask] Fix frames of forces and velocities in foot force difference control #290

Merged
merged 1 commit into from
Sep 9, 2022
Merged

[StabilizerTask] Fix frames of forces and velocities in foot force difference control #290

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 11 additions & 6 deletions src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1142,15 +1142,19 @@ void StabilizerTask::updateFootForceDifferenceControl()
return;
}

sva::PTransformd T_0_L(leftFootTask->surfacePose().rotation());
sva::PTransformd T_0_R(rightFootTask->surfacePose().rotation());
Copy link
Member Author

Choose a reason for hiding this comment

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

I start to consider these lines should be

sva::PTransformd T_0_L = leftFootTask->surfacePose();
sva::PTransformd T_0_R = rightFootTask->surfacePose();

for wrench transformation although we used only the rotation in the similar case https://github.com/arntanguy/lipm_walking_controller/pull/2/files#diff-86694886f91b285c323e710f1b64ec1c0f5ab403d0ee748e112030f766fcae11R139

I'm not confident, so could anyone give some idea?

Copy link
Member Author

Choose a reason for hiding this comment

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

I reconsidered and came to the conclusion that it would be OK to leave it as it is ("as is" in the merged PR).
For wrench, only force is used (moment is not used), so the position of the transformation is not important.
For velocity, the angular velocity is set to zero as shown below, which implicitly assumes a frame whose position is identical to the surface and whose orientation is identical to the inertial frame. In such cases, it is correct to apply rotation-only transformations.
https://github.com/jrl-umi3218/mc_rtc/pull/290/files/72f45b20f3766351b296e37b734b65113184baeb#diff-be940a972e16470195608839479a680e1f8aba715c2e5f4c62cdaa4666be420eR1169-R1170


// In what follows, vertical foot forces are expressed in their respective
// foot sole frames, but foot force difference control expects them to be
// written in the world frame, so the following lines are wrong (they miss a
// frame transform). Thanks to @Saeed-Mansouri for pointing out this bug
// <https://github.com/stephane-caron/lipm_walking_controller/discussions/72>.
double LFz_d = leftFootTask->targetWrench().force().z();
double RFz_d = rightFootTask->targetWrench().force().z();
double LFz = leftFootTask->measuredWrench().force().z();
double RFz = rightFootTask->measuredWrench().force().z();
// T_0_{L/R}.transMul transforms a ForceVecd variable from surface frame to world frame
double LFz_d = T_0_L.transMul(leftFootTask->targetWrench()).force().z();
double RFz_d = T_0_R.transMul(rightFootTask->targetWrench()).force().z();
double LFz = T_0_L.transMul(leftFootTask->measuredWrench()).force().z();
double RFz = T_0_R.transMul(rightFootTask->measuredWrench()).force().z();
dfzForceError_ = (LFz_d - RFz_d) - (LFz - RFz);

double LTz_d = leftFootTask->targetPose().translation().z();
Expand All @@ -1164,8 +1168,9 @@ void StabilizerTask::updateFootForceDifferenceControl()
double dz_vdc = c_.vdcFrequency * vdcHeightError_;
sva::MotionVecd velF = {{0., 0., 0.}, {0., 0., dz_ctrl}};
sva::MotionVecd velT = {{0., 0., 0.}, {0., 0., dz_vdc}};
leftFootTask->refVelB(0.5 * (velT - velF));
rightFootTask->refVelB(0.5 * (velT + velF));
// T_0_{L/R} transforms a MotionVecd variable from world frame to surface frame
leftFootTask->refVelB(0.5 * (T_0_L * (velT - velF)));
rightFootTask->refVelB(0.5 * (T_0_R * (velT + velF)));
}

template Eigen::Vector3d StabilizerTask::computeCoMOffset<&StabilizerTask::ExternalWrench::target>(
Expand Down