Admittance controller for an operational point wrt to a force sensor. It can be a point of the model (hand) or not (created operational point: an object in the hand of the robot) Which is closed to a force sensor (for instance the right or left wrist ft) More...
#include <admittance-control-op-point.hh>
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | AdmittanceControlOpPoint (const std::string &name) |
DECLARE_SIGNAL_IN (dqSaturation, dynamicgraph::Vector) | |
Value of the saturation to apply on the velocity output. More... | |
DECLARE_SIGNAL_IN (force, dynamicgraph::Vector) | |
6d force given by the sensor in its local frame More... | |
DECLARE_SIGNAL_IN (Kd, dynamicgraph::Vector) | |
Derivative gain (6d) for the error on the force. More... | |
DECLARE_SIGNAL_IN (Kp, dynamicgraph::Vector) | |
Gain (6d) for the integration of the error on the force. More... | |
DECLARE_SIGNAL_IN (opPose, dynamicgraph::sot::MatrixHomogeneous) | |
Current position (matrixHomogeneous) of the given operational point. More... | |
DECLARE_SIGNAL_IN (sensorPose, dynamicgraph::sot::MatrixHomogeneous) | |
Current position (matrixHomogeneous) of the given force sensor. More... | |
DECLARE_SIGNAL_IN (w_forceDes, dynamicgraph::Vector) | |
6d desired force of the end-effector in the world frame More... | |
DECLARE_SIGNAL_INNER (w_dq, dynamicgraph::Vector) | |
Internal intergration computed in the world frame. More... | |
DECLARE_SIGNAL_INNER (w_force, dynamicgraph::Vector) | |
6d force given by the sensor in the world frame More... | |
DECLARE_SIGNAL_OUT (dq, dynamicgraph::Vector) | |
Velocity reference for the end-effector in the local frame. More... | |
virtual void | display (std::ostream &os) const |
void | init (const double &dt) |
Initialize the entity. More... | |
void | resetDq () |
Reset the velocity. More... | |
Public Member Functions inherited from dynamicgraph::Entity | |
std::ostream & | displaySignalList (std::ostream &os) const |
Entity (const std::string &name) | |
virtual const std::string & | getClassName () const |
const std::string & | getCommandList () const |
virtual std::string | getDocString () const |
LoggerVerbosity | getLoggerVerbosityLevel () |
LoggerVerbosity | getLoggerVerbosityLevel () |
const std::string & | getName () const |
command::Command * | getNewStyleCommand (const std::string &cmdName) |
CommandMap_t | getNewStyleCommandMap () |
SignalBase< sigtime_t > & | getSignal (const std::string &signalName) |
const SignalBase< sigtime_t > & | getSignal (const std::string &signalName) const |
SignalMap | getSignalMap () const |
double | getStreamPrintPeriod () |
double | getStreamPrintPeriod () |
double | getTimeSample () |
double | getTimeSample () |
bool | hasSignal (const std::string &signame) const |
Logger & | logger () |
Logger & | logger () |
const Logger & | logger () const |
const Logger & | logger () const |
void | sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const std::string &lineId="") |
void | sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const std::string &lineId="") |
void | setLoggerVerbosityLevel (LoggerVerbosity lv) |
void | setLoggerVerbosityLevel (LoggerVerbosity lv) |
bool | setStreamPrintPeriod (double t) |
bool | setStreamPrintPeriod (double t) |
bool | setTimeSample (double t) |
bool | setTimeSample (double t) |
virtual SignalBase< sigtime_t > * | test () |
virtual void | test2 (SignalBase< sigtime_t > *) |
virtual std::ostream & | writeCompletionList (std::ostream &os) const |
virtual std::ostream & | writeGraph (std::ostream &os) const |
virtual | ~Entity () |
Protected Attributes | |
double | m_dt |
Time step of the control. More... | |
bool | m_initSucceeded |
True if the entity has been successfully initialized. More... | |
double | m_mass |
size_type | m_n |
Dimension of the force signals and of the output. More... | |
dynamicgraph::Vector | m_w_dq |
Internal state. More... | |
Protected Attributes inherited from dynamicgraph::Entity | |
CommandMap_t | commandMap |
Logger | logger_ |
std::string | name |
SignalMap | signalMap |
Private Member Functions | |
DYNAMIC_GRAPH_ENTITY_DECL () | |
Additional Inherited Members | |
Public Types inherited from dynamicgraph::Entity | |
typedef std::map< const std::string, command::Command * > | CommandMap_t |
typedef std::map< std::string, SignalBase< sigtime_t > * > | SignalMap |
Protected Member Functions inherited from dynamicgraph::Entity | |
void | addCommand (const std::string &name, command::Command *command) |
void | entityDeregistration () |
void | entityRegistration () |
void | signalDeregistration (const std::string &name) |
void | signalRegistration (const SignalArray< sigtime_t > &signals) |
Admittance controller for an operational point wrt to a force sensor. It can be a point of the model (hand) or not (created operational point: an object in the hand of the robot) Which is closed to a force sensor (for instance the right or left wrist ft)
This entity computes a velocity reference for an operational point based on the force error in the world frame : w_dq = integral(Kp(w_forceDes-w_force)) + Kd (w_dq)
Definition at line 59 of file admittance-control-op-point.hh.
dynamicgraph::sot::core::AdmittanceControlOpPoint::AdmittanceControlOpPoint | ( | const std::string & | name | ) |
Definition at line 55 of file admittance-control-op-point.cpp.
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_IN | ( | dqSaturation | , |
dynamicgraph::Vector | |||
) |
Value of the saturation to apply on the velocity output.
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_IN | ( | force | , |
dynamicgraph::Vector | |||
) |
6d force given by the sensor in its local frame
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_IN | ( | Kd | , |
dynamicgraph::Vector | |||
) |
Derivative gain (6d) for the error on the force.
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_IN | ( | Kp | , |
dynamicgraph::Vector | |||
) |
Gain (6d) for the integration of the error on the force.
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_IN | ( | opPose | , |
dynamicgraph::sot::MatrixHomogeneous | |||
) |
Current position (matrixHomogeneous) of the given operational point.
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_IN | ( | sensorPose | , |
dynamicgraph::sot::MatrixHomogeneous | |||
) |
Current position (matrixHomogeneous) of the given force sensor.
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_IN | ( | w_forceDes | , |
dynamicgraph::Vector | |||
) |
6d desired force of the end-effector in the world frame
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_INNER | ( | w_dq | , |
dynamicgraph::Vector | |||
) |
Internal intergration computed in the world frame.
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_INNER | ( | w_force | , |
dynamicgraph::Vector | |||
) |
6d force given by the sensor in the world frame
dynamicgraph::sot::core::AdmittanceControlOpPoint::DECLARE_SIGNAL_OUT | ( | dq | , |
dynamicgraph::Vector | |||
) |
Velocity reference for the end-effector in the local frame.
|
virtual |
Reimplemented from dynamicgraph::Entity.
Definition at line 199 of file admittance-control-op-point.cpp.
|
private |
void dynamicgraph::sot::core::AdmittanceControlOpPoint::init | ( | const double & | dt | ) |
Initialize the entity.
[in] | dt | Time step of the control |
Definition at line 80 of file admittance-control-op-point.cpp.
void dynamicgraph::sot::core::AdmittanceControlOpPoint::resetDq | ( | ) |
Reset the velocity.
Definition at line 106 of file admittance-control-op-point.cpp.
|
protected |
Time step of the control.
Definition at line 117 of file admittance-control-op-point.hh.
|
protected |
True if the entity has been successfully initialized.
Definition at line 113 of file admittance-control-op-point.hh.
|
protected |
Definition at line 119 of file admittance-control-op-point.hh.
|
protected |
Dimension of the force signals and of the output.
Definition at line 111 of file admittance-control-op-point.hh.
|
protected |
Internal state.
Definition at line 115 of file admittance-control-op-point.hh.