Aikido
aikido::control::KinematicSimulationTrajectoryExecutor Class Reference

Executes trajectories in DART. More...

#include <aikido/control/KinematicSimulationTrajectoryExecutor.hpp>

Inheritance diagram for aikido::control::KinematicSimulationTrajectoryExecutor:
aikido::control::TrajectoryExecutor aikido::control::Executor

Public Member Functions

 KinematicSimulationTrajectoryExecutor (::dart::dynamics::MetaSkeletonPtr metaskeleton)
 Constructor. More...
 
virtual ~KinematicSimulationTrajectoryExecutor ()
 
void validate (const trajectory::Trajectory *traj) override
 Validate the traj in preparation for execution. More...
 
std::future< void > execute (const trajectory::ConstTrajectoryPtr &traj) override
 Execute traj and set future upon completion. More...
 
void step (const std::chrono::system_clock::time_point &timepoint) override
 Step to a point in time. More...
 
void cancel () override
 Cancels the current trajectory. More...
 
- Public Member Functions inherited from aikido::control::TrajectoryExecutor
 TrajectoryExecutor (const std::vector< dart::dynamics::DegreeOfFreedom * > &dofs, const std::set< ExecutorType > otherTypes=std::set< ExecutorType >(), const std::chrono::milliseconds threadRate=defaultThreadRate)
 Constructor Documentation Inherited. More...
 
virtual ~TrajectoryExecutor ()=default
 
- Public Member Functions inherited from aikido::control::Executor
 Executor (const std::set< ExecutorType > &types, const std::vector< dart::dynamics::DegreeOfFreedom * > &dofs, const std::chrono::milliseconds threadRate=defaultThreadRate)
 Constructor. More...
 
 Executor (const ExecutorType type, const std::vector< dart::dynamics::DegreeOfFreedom * > &dofs, std::chrono::milliseconds threadRate=defaultThreadRate)
 Constructor. More...
 
virtual ~Executor ()
 
std::set< ExecutorTypegetTypes () const
 Get all of this Executor's ExecutorTypes. More...
 
const std::vector< dart::dynamics::DegreeOfFreedom * > getDofs () const
 Get list of dofs needed by this Executor. More...
 
void start ()
 Start the underlying ExecutorThread. More...
 
void stop ()
 Stops the underlying ExecutorThread. More...
 
bool registerDofs ()
 Lock the resources required by the DoFs. More...
 
void releaseDofs ()
 Unlock any resources required by the DoFs. More...
 

Private Attributes

::dart::dynamics::MetaSkeletonPtr mMetaSkeleton
 MetaSkeleton to execute trajectories on. More...
 
trajectory::ConstTrajectoryPtr mTraj
 Trajectory being executed. More...
 
statespace::dart::ConstMetaSkeletonStateSpacePtr mStateSpace
 Trajectory's MetaSkeletonStateSpace. More...
 
bool mInProgress
 Whether a trajectory is being executed. More...
 
std::unique_ptr< std::promise< void > > mPromise
 Promise whose future is returned by execute() More...
 
std::mutex mMutex
 Manages access to mTraj, mInProgress, mPromise. More...
 

Additional Inherited Members

- Protected Attributes inherited from aikido::control::TrajectoryExecutor
std::set< const trajectory::Trajectory * > mValidatedTrajectories
 Set of trajectories validated by executor. More...
 
std::chrono::system_clock::time_point mExecutionStartTime
 Time of previous call. More...
 
- Protected Attributes inherited from aikido::control::Executor
std::set< ExecutorTypemTypes
 Vector of executor types. More...
 
std::vector< dart::dynamics::DegreeOfFreedom * > mDofs
 Vector of dof names. More...
 

Detailed Description

Executes trajectories in DART.

This simulates trajectories by setting interpolated DOF positions, without running dynamic simulation.

Constructor & Destructor Documentation

◆ KinematicSimulationTrajectoryExecutor()

aikido::control::KinematicSimulationTrajectoryExecutor::KinematicSimulationTrajectoryExecutor ( ::dart::dynamics::MetaSkeletonPtr  metaskeleton)
explicit

Constructor.

Parameters
metaskeletonMetaSkeleton to execute trajectories on. All trajectories must have dofs only in this skeleton.

◆ ~KinematicSimulationTrajectoryExecutor()

virtual aikido::control::KinematicSimulationTrajectoryExecutor::~KinematicSimulationTrajectoryExecutor ( )
virtual

Member Function Documentation

◆ cancel()

void aikido::control::KinematicSimulationTrajectoryExecutor::cancel ( )
overridevirtual

Cancels the current trajectory.

Implements aikido::control::TrajectoryExecutor.

◆ execute()

std::future<void> aikido::control::KinematicSimulationTrajectoryExecutor::execute ( const trajectory::ConstTrajectoryPtr traj)
overridevirtual

Execute traj and set future upon completion.

Parameters
trajTrajectory to be executed. Its StateSpace should be a MetaSkeletonStateSpace, where the dofs are all in the skeleton passed to this constructor.
Returns
future<void> for trajectory execution. If trajectory terminates before completion, future will be set to a runtime_error.
Exceptions
invalid_argumentif traj is invalid.

Implements aikido::control::TrajectoryExecutor.

◆ step()

void aikido::control::KinematicSimulationTrajectoryExecutor::step ( const std::chrono::system_clock::time_point &  timepoint)
overridevirtual

Step to a point in time.

Note
timepoint can be a time in the future to enable faster than real-time execution.
Parameters
timepointTime to simulate to

If multiple threads are accessing this function or the skeleton associated with this executor, it is necessary to lock the skeleton before calling this method.

Implements aikido::control::TrajectoryExecutor.

◆ validate()

void aikido::control::KinematicSimulationTrajectoryExecutor::validate ( const trajectory::Trajectory traj)
overridevirtual

Validate the traj in preparation for execution.

Parameters
trajTrajectory to be validated

Implements aikido::control::TrajectoryExecutor.

Member Data Documentation

◆ mInProgress

bool aikido::control::KinematicSimulationTrajectoryExecutor::mInProgress
private

Whether a trajectory is being executed.

◆ mMetaSkeleton

::dart::dynamics::MetaSkeletonPtr aikido::control::KinematicSimulationTrajectoryExecutor::mMetaSkeleton
private

MetaSkeleton to execute trajectories on.

◆ mMutex

std::mutex aikido::control::KinematicSimulationTrajectoryExecutor::mMutex
mutableprivate

Manages access to mTraj, mInProgress, mPromise.

◆ mPromise

std::unique_ptr<std::promise<void> > aikido::control::KinematicSimulationTrajectoryExecutor::mPromise
private

Promise whose future is returned by execute()

◆ mStateSpace

statespace::dart::ConstMetaSkeletonStateSpacePtr aikido::control::KinematicSimulationTrajectoryExecutor::mStateSpace
private

Trajectory's MetaSkeletonStateSpace.

◆ mTraj

trajectory::ConstTrajectoryPtr aikido::control::KinematicSimulationTrajectoryExecutor::mTraj
private

Trajectory being executed.