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

Add the possibility to set the SwingFootPlanner position and orientation tolerances as configuration parameters #785

Merged
merged 2 commits into from
Dec 18, 2023
Merged

Add the possibility to set the SwingFootPlanner position and orientation tolerances as configuration parameters #785

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
938565d
Add the possibility to set the SwingFootPlanner position and orientat…
GiulioRomualdi Dec 18, 2023
d0b7b30
Fix contact list comparison bug in SwingFootPlanner
GiulioRomualdi Dec 18, 2023
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
46 changes: 25 additions & 21 deletions src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,8 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
maximum height of the foot. If m_footApexTime is set to 0.5 the
stepHeight is the maximum of the trajectory. */
double m_footApexTime{0.5}; /**< Number between 0 and 1 representing the foot apex instant */
double m_positionTolerance{1e-6}; /**< Position tolerance in \f$m\f$ */
double m_orientationTolerance{1e-6}; /**< Orientation tolerance in \f$rad\f$ */

double m_footLandingVelocity{0.0}; /**< Landing velocity in \f$m/s\f$ */
double m_footLandingAcceleration{0.0}; /**< Landing acceleration in \f$m/s^2\f$ */
Expand Down Expand Up @@ -114,30 +116,27 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
const manif::SO3d::Tangent& initialAngularAcceleration);

public:
// clang-format off
/**
* Initialize the planner.
* @param handler pointer to the parameter handler.
* @note the following parameters are required by the class
* | Parameter Name | Type | Description | Mandatory |
* | Parameter Name | Type | Description | Mandatory |
* |:----------------------------:|:--------:|:----------------------------------------------------------------------------------------------------------------------------------------------------------:|:---------:|
* | `sampling_time` | `double` | Sampling time of the planner in seconds | Yes
* | | `step_height` | `double` | Height of the
* swing foot. It is not the maximum height of the foot. If apex time is 0.5 `step_height` is
* the maximum | Yes | | `foot_apex_time` | `double` | Number between
* 0 and 1 representing the foot apex instant. If 0 the apex happens at take off if 1 at touch
* down | Yes | | `foot_landing_velocity` | `double`
* | Landing vertical velocity (default value 0.0) | No
* | | `foot_landing_acceleration` | `double` | Landing
* vertical acceleration (default value 0.0) | No | | `foot_take_off_velocity` |
* `double` | Take-off vertical velocity (default value
* 0.0) | No | |
* `foot_take_off_acceleration` | `double` | Take-off
* vertical acceleration (default value 0.0) | No | | `interpolation_method` |
* `string` | Define the interpolation method for the trajectory of the position. Accepted
* parameters: `min_acceleration`, `min_jerk` (default value `min_acceleration`) | No |
* | `sampling_time` | `double` | Sampling time of the planner in seconds | Yes |
* | `step_height` | `double` | Height of the swing foot. It is not the maximum height of the foot. If apex time is 0.5 `step_height` is the maximum | Yes |
* | `foot_apex_time` | `double` | Number between 0 and 1 representing the foot apex instant. If 0 the apex happens at take off if 1 at touch down | Yes |
* | `foot_landing_velocity` | `double` | Landing vertical velocity (default value 0.0) | No |
* | `foot_landing_acceleration` | `double` | Landing vertical acceleration (default value 0.0) | No |
* | `foot_take_off_velocity` | `double` | Take-off vertical velocity (default value 0.0) | No |
* | `foot_take_off_acceleration` | `double` | Take-off vertical acceleration (default value 0.0) | No |
* | `interpolation_method` | `string` | Define the interpolation method for the trajectory of the position. Accepted parameters: `min_acceleration`, `min_jerk` (default value `min_acceleration`) | No |
* | `position_tolerance` | `double` | Position tolerance in meters considered in SwingFootPlanner::setContactList. (default value 1e-6) | No |
* | `orientation_tolerance` | `double` | Orientation tolerance in radians considered in SwingFootPlanner::setContactList. (default value 1e-6) | No |
* @return True in case of success/false otherwise.
*/
bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) final;
// clang-format on

/**
* Set the contact list
Expand All @@ -151,15 +150,20 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
* limitations of the framework (mainly due to the SO3 trajectory generation) for the time
* being, we support only the two following cases:
* - Given the current time instant, both the stored and the new contact lists must have an
* active contact at the same pose.
* active contact at the same pose. In details the check is done by comparing the position and
* orientation of the contact. The position and orientation are considered equal if the
* following two conditions are satisfied:
* 1. the distance between the two positions is less than position_tolerance
* 2. the distance between the two orientations is less than orientation_tolerance
* - If the contact is not active (swing phase) the next contact must satisfy the following two
* hypothesis
* 1. the final orientation may change still the error (in the tangent space) between the
* new orientation and the current one should be parallel to the current velocity and
* acceleration vectors. This is required to keep the SO3Planner problem still treatable
* online. This check is not done here since the SO3Planner will complain in case of issues.
* new orientation and the current one should be parallel to the current velocity and
* acceleration vectors. This is required to keep the SO3Planner problem still treatable
* online. This check is not done here since the SO3Planner will complain in case of
* issues.
* 2. the impact time of the next contact must be the same as the one of the next contact in
* the contact list stored in the class.
* the contact list stored in the class.
*/
bool setContactList(const Contacts::ContactList& contactList);

Expand Down
27 changes: 24 additions & 3 deletions src/Planners/src/SwingFootPlanner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,18 @@ bool SwingFootPlanner::initialize(std::weak_ptr<const IParametersHandler> handle
m_footLandingAcceleration);
}

if (!ptr->getParameter("position_tolerance", m_positionTolerance))
{
log()->info("{} Using default position_tolerance={} m.", logPrefix, m_positionTolerance);
}

if (!ptr->getParameter("orientation_tolerance", m_orientationTolerance))
{
log()->info("{} Using default orientation_tolerance={} rad.",
logPrefix,
m_orientationTolerance);
}

// check the parameters passed to the planner
const bool ok = (m_dT > std::chrono::nanoseconds::zero())
&& ((m_footApexTime > 0) && (m_footApexTime < 1));
Expand Down Expand Up @@ -157,23 +169,32 @@ bool SwingFootPlanner::setContactList(const ContactList& contactList)
auto activeContactNewList = contactList.getActiveContact(m_currentTrajectoryTime);
auto activeContact = m_contactList.getActiveContact(m_currentTrajectoryTime);

constexpr double tolerance = 1e-6;
if (activeContact != m_contactList.cend() // i.e., the original list contains an active contact
&& activeContactNewList != contactList.cend() // i.e., the new list contains an active
// contact
)
{
if (!(activeContact->pose - activeContactNewList->pose).coeffs().isZero(tolerance))
if (!(activeContact->pose.translation() - activeContactNewList->pose.translation())
.isZero(m_positionTolerance)
|| (activeContact->pose.asSO3() - activeContactNewList->pose.asSO3()).coeffs().norm()
> m_orientationTolerance)
{
log()->error("{} The pose of the contact in the new contact list is different from the "
"pose of the contact in the original contact list. Given the contact "
"lists and the time instant the contacts are active."
"Original contact: {}, new contact: {}. Error {}. Current time {}.",
"Original contact: {}, new contact: {}. Error {}. Admissible position "
"tolerance "
"{} m, admissible orientation tolerance {} rad. Current time {}.",
logPrefix,
activeContact->pose.coeffs().transpose(),
activeContactNewList->pose.coeffs().transpose(),
(activeContactNewList->pose - activeContact->pose).coeffs().transpose(),
m_positionTolerance,
m_orientationTolerance,
toMilliseconds(m_currentTrajectoryTime));
log()->info("{} Current contact list.{}", logPrefix, m_contactList.toString());
log()->info("{} New contact list. {}", logPrefix, contactList.toString());

return false;
}

Expand Down
Loading