List of all members | Public Member Functions | Public Attributes
TSMP Class Reference

#include <armarx/control/common/mp/TSMP.h>

+ Inheritance diagram for TSMP:

Public Member Functions

void computePhaseStop (const MPInputPtr input, const MPOutputPtr output, PhaseStopResult &phase) override
 
void run (MPInputPtr, MPOutputPtr, const PhaseStopResult &phase) override
 
 TSMP (const MPConfig &c)
 
void validateInitialState (const DVec &starts) override
 
DVec validateViaPoint (const DVec &viapoint) override
 
 ~TSMP ()
 
- Public Member Functions inherited from MP
DVec deserialize (const std::string &, const std::string &mode="string")
 
double getCanonicalValue ()
 
const std::string & getClassName () const
 
MPConfig getConfig ()
 
const arondto::MPGroup & getGroup () const
 
std::string getGroupName () const
 
const std::string & getMPName () const
 
const std::string & getNodeSetName () const
 
const std::string & getRole () const
 
bool getStartFromPrevTarget ()
 
std::vector< double > getStartVec ()
 
DVecVec getWeight ()
 
bool isFinished ()
 status
 
bool isFirstRun ()
 
bool isRunning ()
 
bool isStopRequested ()
 
bool isTaskSpaceMP () const
 
void learnFromCSV (const Ice::StringSeq &fileNames=std::vector< std::string >())
 setting
 
void learnFromTraj (const MPTrajs &trajs=MPTrajs())
 
 MP (const MPConfig &c)
 
void pause ()
 
void removeAllViaPoint ()
 
void requestStop ()
 
void reset ()
 
void resume ()
 
std::string serialize (const std::string &mode="string")
 
void setDurationSec (Ice::Double timeDuration)
 
void setGoal (const DVec &goals)
 
void setRotationWeights (const DVecVec &weights)
 
void setStart (const DVec &starts)
 
void setStartAndGoal (const DVec &starts, const DVec &goals)
 
void setTranslationWeights (const DVecVec &weights)
 
void setViaPoint (Ice::Double u, const DVec &viapoint)
 
void setWeight (const DVecVec &weights)
 serialze
 
void start ()
 control
 
void start (const DVec &goals)
 
void start (const DVec &goals, const DVec &starts)
 
void start (const DVec &goals, const DVec &starts, Ice::Double timeDuration)
 
void start (const DVec &goals, Ice::Double timeDuration)
 
void stop ()
 
void trainMP ()
 
void trainMPFromTraj (std::vector< mplib::core::SampledTrajectory > &trajs)
 
virtual ~MP ()=default
 

Public Attributes

bool isDisturbance = false
 
Eigen::Quaterniond previousAngularVelocity = Eigen::Quaterniond::Identity()
 
- Public Attributes inherited from MP
DVec prevViapoint
 

Additional Inherited Members

- Protected Attributes inherited from MP
double canonicalValue = 1.0
 
MPConfig cfg
 
std::vector< mplib::representation::MPState > currentState
 
std::atomic_bool finished = false
 
std::atomic_bool firstRun = true
 
std::atomic_bool goalSetByUser {false}
 
bool isDisturbance = false
 
MutexType mpMutex
 
armarx::DateTime mpStartTimeSec
 
std::atomic_bool mpTrained {false}
 Note: if mp is not trained, the dimension is not initialized, thus mp->getDim() is undefined, if you set viapoints you will have memory allocation error.
 
std::atomic_bool paused = false
 
std::atomic_bool running = false
 
std::atomic_bool stopRequested = false
 
DVec targetPoseVec
 
DVec targetPoseVecInTraj
 
std::vector< std::pair< double, DVec > > userDefinedViaPoints
 
VMPPtr vmp
 
mplib::representation::VMPType vmpType = mplib::representation::VMPType::PrincipalComponent
 

Detailed Description

Definition at line 49 of file TSMP.h.

Constructor & Destructor Documentation

◆ TSMP()

TSMP ( const MPConfig & c)

Definition at line 15 of file TSMP.cpp.

+ Here is the call graph for this function:

◆ ~TSMP()

~TSMP ( )
inline

Definition at line 53 of file TSMP.h.

Member Function Documentation

◆ computePhaseStop()

void computePhaseStop ( const MPInputPtr input,
const MPOutputPtr output,
PhaseStopResult & phase )
overridevirtual

Necessary for the D-term in Phase Stop

Reimplemented from MP.

Definition at line 55 of file TSMP.cpp.

+ Here is the call graph for this function:

◆ run()

void run ( MPInputPtr input,
MPOutputPtr output,
const PhaseStopResult & phase )
overridevirtual

Necessary for the D-term in Phase Stop

Sync internal state with current robot pose

Time Evolution (Canonical Value) and Step MP forward

scale translation velocity

Velocity Blending (Translation)

PD Controller pulling towards target

Blend: Trust MP if mpcFactor is 1, Trust PD if mpcFactor is 0

Velocity Blending (Orientation)

Compute MP angular velocity

Check for quaternion flip (continuity)

Scale by amplitude/duration

Update internal state

Orientation PD

Blend

Output Pose

Flatten targetState to vector and normalize quaternion

Compute norm of the last 4 elements

Implements MP.

Definition at line 106 of file TSMP.cpp.

+ Here is the call graph for this function:

◆ validateInitialState()

void validateInitialState ( const DVec & starts)
overridevirtual

Reimplemented from MP.

Definition at line 35 of file TSMP.cpp.

+ Here is the call graph for this function:

◆ validateViaPoint()

DVec validateViaPoint ( const DVec & viapoint)
overridevirtual

Reimplemented from MP.

Definition at line 43 of file TSMP.cpp.

+ Here is the call graph for this function:

Member Data Documentation

◆ isDisturbance

bool isDisturbance = false

Definition at line 64 of file TSMP.h.

◆ previousAngularVelocity

Eigen::Quaterniond previousAngularVelocity = Eigen::Quaterniond::Identity()

Definition at line 62 of file TSMP.h.

The documentation for this class was generated from the following files: