Aikido
aikido::control::VisualServoingVelocityExecutor Class Reference

Executes end-effector R3 linear velocity command towards a perceived target Uses underlying JacobianVelocityExecutor on its own thread. More...

#include <aikido/control/VisualServoingVelocityExecutor.hpp>

Inheritance diagram for aikido::control::VisualServoingVelocityExecutor:
aikido::control::Executor

Classes

struct  Properties
 Paramters for Vector Field Planner. More...
 

Public Member Functions

 VisualServoingVelocityExecutor (::dart::dynamics::BodyNode *eeNode, std::shared_ptr< JacobianVelocityExecutor > executor=nullptr, Properties properties=Properties())
 Constructor. More...
 
virtual ~VisualServoingVelocityExecutor ()
 
void resetProperties (Properties properties=Properties())
 Cancel current command and update properties. More...
 
Properties getProperties () const
 Get current properties. More...
 
virtual std::future< int > execute (std::function< std::shared_ptr< Eigen::Isometry3d >(void)> perception)
 Execute a Visual Servoing Command, setting future upon completion. More...
 
virtual std::future< int > execute (std::function< std::shared_ptr< Eigen::Isometry3d >(void)> perception, const std::chrono::system_clock::time_point &timepoint)
 
void step (const std::chrono::system_clock::time_point &timepoint) override
 Step to a point in time. More...
 
void cancel ()
 Cancels the current command. More...
 
- 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::BodyNode * mEENode
 End Effector Body Node Name. More...
 
std::shared_ptr< JacobianVelocityExecutormExecutor
 Underlying JacobianVelocityExecutor. More...
 
std::function< std::shared_ptr< Eigen::Isometry3d >void)> mPerception
 Perception Function (if empty, then executor not in progress) More...
 
std::unique_ptr< std::promise< int > > mPromise
 Promise whose future is returned by execute() More...
 
std::mutex mMutex
 Manages access to mPerception, mPromise. More...
 
Properties mProperties
 Properties (see documentation above) More...
 
std::future< int > mFuture
 Future for underlying executor call. More...
 
std::chrono::system_clock::time_point mLastPerceivedTime
 Time of command start. More...
 

Additional Inherited Members

- 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 end-effector R3 linear velocity command towards a perceived target Uses underlying JacobianVelocityExecutor on its own thread.

Constructor & Destructor Documentation

◆ VisualServoingVelocityExecutor()

aikido::control::VisualServoingVelocityExecutor::VisualServoingVelocityExecutor ( ::dart::dynamics::BodyNode *  eeNode,
std::shared_ptr< JacobianVelocityExecutor executor = nullptr,
Properties  properties = Properties() 
)
explicit

Constructor.

Parameters
eeNodeEnd effector body node (current position = getWorldTransform())
executorBase jacobian velocity executor. Default: kinematic, constructed from BodyNode
propertiesSee ::Properties for more info.

◆ ~VisualServoingVelocityExecutor()

virtual aikido::control::VisualServoingVelocityExecutor::~VisualServoingVelocityExecutor ( )
virtual

Member Function Documentation

◆ cancel()

void aikido::control::VisualServoingVelocityExecutor::cancel ( )
virtual

Cancels the current command.

Reimplemented from aikido::control::Executor.

◆ execute() [1/2]

virtual std::future<int> aikido::control::VisualServoingVelocityExecutor::execute ( std::function< std::shared_ptr< Eigen::Isometry3d >(void)>  perception)
inlinevirtual

Execute a Visual Servoing Command, setting future upon completion.

Note
Future should return 0 on success or timeout.
Parameters
perceptiongoal position in DART World Frame

◆ execute() [2/2]

virtual std::future<int> aikido::control::VisualServoingVelocityExecutor::execute ( std::function< std::shared_ptr< Eigen::Isometry3d >(void)>  perception,
const std::chrono::system_clock::time_point &  timepoint 
)
virtual

◆ getProperties()

Properties aikido::control::VisualServoingVelocityExecutor::getProperties ( ) const

Get current properties.

◆ resetProperties()

void aikido::control::VisualServoingVelocityExecutor::resetProperties ( Properties  properties = Properties())

Cancel current command and update properties.

◆ step()

void aikido::control::VisualServoingVelocityExecutor::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.

Reimplemented from aikido::control::Executor.

Member Data Documentation

◆ mEENode

::dart::dynamics::BodyNode* aikido::control::VisualServoingVelocityExecutor::mEENode
private

End Effector Body Node Name.

◆ mExecutor

std::shared_ptr<JacobianVelocityExecutor> aikido::control::VisualServoingVelocityExecutor::mExecutor
private

Underlying JacobianVelocityExecutor.

◆ mFuture

std::future<int> aikido::control::VisualServoingVelocityExecutor::mFuture
private

Future for underlying executor call.

◆ mLastPerceivedTime

std::chrono::system_clock::time_point aikido::control::VisualServoingVelocityExecutor::mLastPerceivedTime
private

Time of command start.

◆ mMutex

std::mutex aikido::control::VisualServoingVelocityExecutor::mMutex
mutableprivate

Manages access to mPerception, mPromise.

◆ mPerception

std::function<std::shared_ptr<Eigen::Isometry3d>void)> aikido::control::VisualServoingVelocityExecutor::mPerception
private

Perception Function (if empty, then executor not in progress)

◆ mPromise

std::unique_ptr<std::promise<int> > aikido::control::VisualServoingVelocityExecutor::mPromise
private

Promise whose future is returned by execute()

◆ mProperties

Properties aikido::control::VisualServoingVelocityExecutor::mProperties
private

Properties (see documentation above)