Skip to content

Commit

Permalink
Restructure the schmitt trigger detector considering f33d477 and 6fbbc4a
Browse files Browse the repository at this point in the history
  • Loading branch information
@GiulioRomualdi
GiulioRomualdi committed Mar 13, 2023
1 parent f33d477 commit 4ca0798
Show file tree
Hide file tree
Showing 3 changed files with 204 additions and 369 deletions.
183 changes: 49 additions & 134 deletions src/Contacts/include/BipedalLocomotion/ContactDetectors/SchmittTriggerDetector.h
View file
Original file line number Diff line number Diff line change
@@ -1,67 +1,75 @@
/**
* @file SchmittTriggerDetector.h
* @authors Prashanth Ramadoss
* @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* @authors Prashanth Ramadoss, Giulio Romualdi
* @copyright 2020-2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#ifndef BIPEDAL_LOCOMOTION_CONTACT_DETECTORS_SCHMITT_TRIGGER_DETECTOR_H
#define BIPEDAL_LOCOMOTION_CONTACT_DETECTORS_SCHMITT_TRIGGER_DETECTOR_H

#include <BipedalLocomotion/ContactDetectors/ContactDetector.h>

#include <iostream>
#include <unordered_map>

#include <BipedalLocomotion/ContactDetectors/ContactDetector.h>
#include <BipedalLocomotion/Math/SchmittTrigger.h>

namespace BipedalLocomotion
{
namespace Contacts
{

struct SchmittTriggerParams;
struct SchmittTriggerInput;

/**
* Schmitt Trigger thresholding based contact detector
* that maintains and updates the contact states for
* a prescribed set of contacts
* Schmitt Trigger thresholding based contact detector that maintains and updates the contact states
* for a prescribed set of contacts.
*/
class SchmittTriggerDetector : public ContactDetector
{
public:

/**
* Constructor.
* It is required by the pimpl idiom.
*/
SchmittTriggerDetector();

/**
* Destructor.
* It is required by the pimpl idiom.
*/
~SchmittTriggerDetector();

/**
* Initialize the SchmittTriggerDetector witn a parameters handler.
* @param[in] handler configure the custom parameters for the detector.
* @note The following parameters are required
* | Parameter Name | Type | Description | Mandatory |
* |:----------------------------:|:----------------:|:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------:|:---------:|
* | `contacts` | `vector<string>` | Vector containing the names of the contact. The name can be used to retrieve the current status of a contact with `ContactDetector::get`. | Yes |
* | `contact_make_thresholds` | `vector<double>` | Vector containing High-value thresholds to initiate an ON state switch after switchOnAfter time-units. For each contact specified in `contacts` the user needs to specify an element of the vector. | Yes |
* | `contact_break_thresholds` | `vector<double>` | Vector containing Low-value thresholds to initiate an ON state switch after switchOnAfter time-units. For each contact specified in `contacts` the user needs to specify an element of the vector. | Yes |
* | `contact_make_switch_times` | `vector<double>` | Time units to wait for before switching to `contact` state from `no-contact` state. For each contact specified in `contacts` the user needs to specify an element of the vector. | Yes |
* | `contact_break_switch_times` | `vector<double>` | Time units to wait for before switching to `no-contact` state from `contact` state. For each contact specified in `contacts` the user needs to specify an element of the vector. | Yes |
* @return True in case of success, false otherwise.
*/
bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;

/**
* Set trigger input and time stamp for an existing SchmittTrigger unit
* @param[in] contactName name of the contact
* @param[in] time time of measurement
* @param[in] force contact force intensity (typically contact normal force)
* @param[in] input the imput of the trigger containing the contact force and the time instant
* @return True in case of success, false otherwise.
*/
bool setTimedTriggerInput(const std::string& contactName,
const double& time,
const double& triggerInput);
bool setTimedTriggerInput(const std::string& contactName, //
const Math::SchmittTriggerInput& input);

/**
* Set trigger input and time stamp for existing units
* @param[in] timedInputs container of timed trigger inputs, pair(first, second): (time, force)
* @note any unit names in the input container that does not already exist will be ignored
* @return True in case of success, false otherwise.
*/
bool setTimedTriggerInputs(const std::unordered_map<std::string, SchmittTriggerInput>& timedInputs);

/**
* Add a contact whose contact state need to be tracked
* @param[in] contactName name of the contact
* @param[in] initialState initial contact state
* @param[in] params Schmitt Trigger parameters
* @note this method does not reset the state and parameters if the contact already exists
* @return True in case of success, false otherwise.
*/
bool addContact(const std::string& contactName,
const bool& initialState,
const SchmittTriggerParams& params);
bool setTimedTriggerInputs(const std::unordered_map<std::string, //
Math::SchmittTriggerInput>& timedInputs);

/**
* Add a contact whose contact state need to be tracked
Expand All @@ -73,18 +81,16 @@ class SchmittTriggerDetector : public ContactDetector
* @return True in case of success, false otherwise.
*/
bool addContact(const std::string& contactName,
const bool& initialState,
const SchmittTriggerParams& params,
const double& time_now);
const BipedalLocomotion::Math::SchmittTriggerState& initialState,
const BipedalLocomotion::Math::SchmittTrigger::Params& params);

/**
* Reset a contact's state
* @param[in] contactName name of the contact
* @param[in] initialState contact state
* @return True in case of success, false if contact does not exist/otherwise.
*/
bool resetState(const std::string& contactName,
const bool& state);
bool resetState(const std::string& contactName, const bool& state);

/**
* Reset a contact's parameters
Expand All @@ -94,124 +100,33 @@ class SchmittTriggerDetector : public ContactDetector
* @return True in case of success, false if contact does not exist/otherwise.
*/
bool resetContact(const std::string& contactName,
const bool& state,
const SchmittTriggerParams& params);
const bool state,
const BipedalLocomotion::Math::SchmittTrigger::Params& params);

/**
* Remove contact from the Detector
* @param[in] contactName name of the contact
* @return True in case of success, false if does not exist/otherwise.
*/
bool removeContact(const std::string& contactName);
protected:
/**
* These custom parameter specifications should be specified by the derived class.
* @param[in] handler configure the custom parameters for the detector
* @return True in case of success, false otherwise.
*/
virtual bool customInitialization(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;

/**
* Update contact states based on thresholding of contact normal forces and timing parameters
* @return True in case of success, false otherwise.
*/
virtual bool updateContactStates() override;
* Compute one step of the detector.
* This method uses all the inputs and evaluate the contact status considering the Schmitt
* trigger output.
* @return True in case of success, false otherwise.
*/
bool advance() final;

private:

/**
* Private implementation of the class
*/
class Impl;
std::unique_ptr<Impl> m_pimpl; /**< Pointer to implementation */
};


/**
* Struct holding switching parameters for the Schmitt Trigger
*/
struct SchmittTriggerParams
{
double onThreshold{0.0}; /**< high value threshold to initiate an ON state switch after switchOnAfter time-units*/
double offThreshold{0.0}; /**< low value threshold to initiate an OFF state switch after switchOffAfter time-units*/
double switchOnAfter{0.0}; /**< time units to wait for before switching to ON state from OFF state. Ensure it's greater than sampling time. */
double switchOffAfter{0.0}; /**< time units to wait for before switching to OFF state from ON state. Ensure it's greater than sampling time. */
};

/**
* Struct holding Schmitt Trigger inputs
*/
struct SchmittTriggerInput
{
double time{0.0}; /**< time stamp*/
double value{0.0}; /**< signal input*/
};

/**
* Schmitt trigger unit that switches state using threshold and timing parameters
*/
class SchmittTriggerUnit
{
public:
/**
* Set current state of the Schmitt trigger
* @param state current state
*/
void setState(const bool& state);

/**
* Set current state of the Schmitt trigger
* @param state current state
* @param time_now time unit to set the timer parameters
*/
void setState(const bool& state, const double& time_now);

/**
* Set configuration parameters of the Schmitt trigger
* @param params struct holding Schmitt trigger parameters
*/
void setParams(const SchmittTriggerParams& params);

/**
* Update the state of Schmitt trigger with the measurements
* @param currentTime time of measurement
* @param rawValue measurement
* @return True in case of success, false otherwise.
*/
void update(const double& currentTime, const double& rawValue);

/**
* Reset the state of Schmitt trigger to false
*/
void reset();

/**
* Get the current state of the Schmitt trigger
* @return state - true/false
*/
bool getState();

/**
* Get the current state of the Schmitt trigger
* @param[out] swtichTime
* @return state - true/false
*/
bool getState(double& switchTime);

/**
* Get currently configuration of Schmitt trigger
* @return struct holding the parameters
*/
SchmittTriggerParams getParams();

private:
SchmittTriggerParams params; /**< Schmitt Trigger parameters*/
bool state{false}; /**< current state*/
double switchTime{0.}; /**> time instant at which the state was toggled */
double previousTime{0.}; /**< previous update time*/
double timer{0.}; /**< elapsed timer for current state*/
double initialTime{0.}; /**< initialization time*/
};

} // namespace Contacts
} // namespace BipedalLocomotion

Expand Down
Loading

0 comments on commit 4ca0798

Please sign in to comment.