Public Member Functions | List of all members
CartesianBot Class Reference

Implements the Cartesian space solver based on the geometrical solution of for the ASIBOT robot. More...

#include <CartesianBot.h>

Collaboration diagram for CartesianBot:
Collaboration graph
[legend]

Public Member Functions

bool fwdKin (const double inDeg[NUM_MOTORS], yarp::sig::Vector &x, yarp::sig::Vector &o)
 
virtual bool setTrackingMode (const bool f)
 
virtual bool getTrackingMode (bool *f)
 
virtual bool setReferenceMode (const bool f)
 
virtual bool getReferenceMode (bool *f)
 
virtual bool setPosePriority (const yarp::os::ConstString &p)
 
virtual bool getPosePriority (yarp::os::ConstString &p)
 
virtual bool getPose (yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
 
virtual bool getPose (const int axis, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
 
virtual bool goToPose (const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0)
 
virtual bool goToPosition (const yarp::sig::Vector &xd, const double t=0.0)
 
virtual bool goToPoseSync (const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
 
virtual bool goToPositionSync (const yarp::sig::Vector &xd, const double t=0.0)
 
virtual bool getDesired (yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
 
virtual bool askForPose (const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
 
virtual bool askForPose (const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
 
virtual bool askForPosition (const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
 
virtual bool askForPosition (const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
 
virtual bool getDOF (yarp::sig::Vector &curDof)
 
virtual bool setDOF (const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
 
virtual bool getRestPos (yarp::sig::Vector &curRestPos)
 
virtual bool setRestPos (const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
 
virtual bool getRestWeights (yarp::sig::Vector &curRestWeights)
 
virtual bool setRestWeights (const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
 
virtual bool getLimits (const int axis, double *min, double *max)
 
virtual bool setLimits (const int axis, const double min, const double max)
 
virtual bool getTrajTime (double *t)
 
virtual bool setTrajTime (const double t)
 
virtual bool getInTargetTol (double *tol)
 
virtual bool setInTargetTol (const double tol)
 
virtual bool getJointsVelocities (yarp::sig::Vector &qdot)
 
virtual bool getTaskVelocities (yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
 
virtual bool setTaskVelocities (const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
 
virtual bool attachTipFrame (const yarp::sig::Vector &x, const yarp::sig::Vector &o)
 
virtual bool getTipFrame (yarp::sig::Vector &x, yarp::sig::Vector &o)
 
virtual bool removeTipFrame ()
 
virtual bool checkMotionDone (bool *f)
 
virtual bool waitMotionDone (const double period=0.1, const double timeout=0.0)
 
virtual bool stopControl ()
 
virtual bool storeContext (int *id)
 
virtual bool restoreContext (const int id)
 
virtual bool getInfo (yarp::os::Bottle &info)
 
virtual bool registerEvent (yarp::dev::CartesianEvent &event)
 
virtual bool unregisterEvent (yarp::dev::CartesianEvent &event)
 
virtual bool tweakSet (const yarp::os::Bottle &options)
 
virtual bool tweakGet (yarp::os::Bottle &options)
 
virtual bool deleteContext (const int id)
 
virtual bool open (Searchable &config)
 
virtual bool close ()
 
bool threadInit ()
 
void run ()
 

Detailed Description

The CartesianBot class connects to a robot (the IPositionControl, IVelocityControl and IEncoders interfaces) and exposes a YARP_dev cartesian interface (implements ICartesianControl). It is used by the cartesianServer module.

Member Function Documentation

bool CartesianBot::askForPose ( const yarp::sig::Vector &  xd,
const yarp::sig::Vector &  od,
yarp::sig::Vector &  xdhat,
yarp::sig::Vector &  odhat,
yarp::sig::Vector &  qdhat 
)
virtual

Ask for inverting a given pose without actually moving there. [wait for reply]

Parameters
xda 3-d vector which contains the desired position x,y,z (meters).
oda 2-d vector which contains the desired orientation using axis-angle representation xa, ya, za, theta (meters and radians).
xdhata 3-d vector which is filled with the final position x,y,z (meters); it may differ from the commanded xd.
odhata 2-d vector which is filled with the final orientation using axis-angle representation xa, ya, za, theta (meters and radians); it may differ from the commanded od.
qdhatthe joints configuration through which the couple (xdhat,odhat) is achieved (degrees).
Returns
true/false on success/failure.
bool CartesianBot::askForPose ( const yarp::sig::Vector &  q0,
const yarp::sig::Vector &  xd,
const yarp::sig::Vector &  od,
yarp::sig::Vector &  xdhat,
yarp::sig::Vector &  odhat,
yarp::sig::Vector &  qdhat 
)
virtual

Ask for inverting a given pose without actually moving there. [wait for reply]

Parameters
q0a vector of length DOF which contains the starting joints configuration (degrees), made compatible with the chain.
xda 3-d vector which contains the desired position x,y,z (meters).
oda 2-d vector which contains the desired orientation using axis-angle representation xa, ya, za, theta (meters and radians).
xdhata 3-d vector which is filled with the final position x,y,z (meters); it may differ from the commanded xd.
odhata 2-d vector which is filled with the final orientation using axis-angle representation xa, ya, za, theta (meters and radians); it may differ from the commanded od.
qdhatthe joints configuration through which the couple (xdhat,odhat) is achieved (degrees).
Returns
true/false on success/failure.
bool CartesianBot::askForPosition ( const yarp::sig::Vector &  xd,
yarp::sig::Vector &  xdhat,
yarp::sig::Vector &  odhat,
yarp::sig::Vector &  qdhat 
)
virtual

Ask for inverting a given position without actually moving there. [wait for reply]

Parameters
xda 3-d vector which contains the desired position x,y,z (meters).
xdhata 3-d vector which is filled with the final position x,y,z (meters); it may differ from the commanded xd.
odhata 2-d vector which is filled with the final orientation using axis-angle representation xa, ya, za, theta (meters and radians); it may differ from the commanded od.
qdhatthe joints configuration through which the couple (xdhat,odhat) is achieved (degrees).
Returns
true/false on success/failure.
bool CartesianBot::askForPosition ( const yarp::sig::Vector &  q0,
const yarp::sig::Vector &  xd,
yarp::sig::Vector &  xdhat,
yarp::sig::Vector &  odhat,
yarp::sig::Vector &  qdhat 
)
virtual

Ask for inverting a given position without actually moving there. [wait for reply]

Parameters
q0a vector of length DOF which contains the starting joints configuration (degrees), made compatible with the chain.
xda 3-d vector which contains the desired position x,y,z (meters).
xdhata 3-d vector which is filled with the final position x,y,z (meters); it may differ from the commanded xd.
odhata 2-d vector which is filled with the final orientation using axis-angle representation xa, ya, za, theta (meters and radians); it may differ from the commanded od.
qdhatthe joints configuration through which the couple (xdhat,odhat) is achieved (degrees).
Returns
true/false on success/failure.
bool CartesianBot::attachTipFrame ( const yarp::sig::Vector &  x,
const yarp::sig::Vector &  o 
)
virtual

Attach a tip frame to the end-effector.

Parameters
xa 3-d vector describing the position of the tip frame wrt the end-effector (meters).
oa 4-d vector describing the orientation of the tip frame wrt the end-effector (axis-angle notation).
Returns
true/false if successful/failed.
Note
By attaching a tip to the end-effector, the specified tip will be the new end-effector for the controller.
bool CartesianBot::checkMotionDone ( bool *  f)
virtual

Check once if the current trajectory is terminated. [wait for reply]

Parameters
fwhere the result is returned.
Returns
true/false on success/failure.
bool CartesianBot::close ( )
virtual

Close the DeviceDriver.

Returns
true/false on success/failure.
bool CartesianBot::deleteContext ( const int  id)
virtual

Delete a specified controller context. [wait for reply]

Parameters
idspecify the context id to be removed.
Returns
true/false on success/failure.
bool CartesianBot::fwdKin ( const double  inDeg[NUM_MOTORS],
yarp::sig::Vector &  x,
yarp::sig::Vector &  o 
)

Perform forward kinematics.

bool CartesianBot::getDesired ( yarp::sig::Vector &  xdhat,
yarp::sig::Vector &  odhat,
yarp::sig::Vector &  qdhat 
)
virtual

Get the actual desired pose and joints configuration as result of kinematic inversion. [wait for reply]

Parameters
xdhata 3-d vector which is filled with the actual desired position x,y,z (meters); it may differ from the commanded xd.
odhata 2-d vector which is filled with the actual desired orientation using axis-angle representation xa, ya, za, theta (meters and radians); it may differ from the commanded od.
qdhatthe joints configuration through which the couple (xdhat,odhat) is achieved (degrees).
Returns
true/false on success/failure.
bool CartesianBot::getDOF ( yarp::sig::Vector &  curDof)
virtual

Get the current DOF configuration of the limb. [wait for reply]

Parameters
curDofa vector which is filled with the actual DOF configuration.
Returns
true/false on success/failure.
Note
The vector lenght is equal to the number of limb's joints; each vector's position is filled with 1 if the associated joint is controlled (i.e. it is an actuated DOF), 0 otherwise.
bool CartesianBot::getInfo ( yarp::os::Bottle &  info)
virtual

Returns useful info on the operating state of the controller. [wait for reply]

Parameters
infois a property-like bottle containing the info.
Returns
true/false on success/failure.
bool CartesianBot::getInTargetTol ( double *  tol)
virtual

Return tolerance for in-target check. [wait for reply]

Parameters
tolthe memory location where tolerance is returned.
Returns
true/false on success/failure.
Note
The trajectory is supposed to be completed as soon as norm(xd-end_effector)<tol.
bool CartesianBot::getJointsVelocities ( yarp::sig::Vector &  qdot)
virtual

Return joints velocities. [wait for reply]

Parameters
qdotthe vector containing the joints velocities [deg/s] sent to the robot by the controller.
Returns
true/false on success/failure.
bool CartesianBot::getLimits ( const int  axis,
double *  min,
double *  max 
)
virtual

Get the current range for the axis. [wait for reply]

Parameters
axisjoint index (regardless if it is actuated or not).
minwhere the minimum value is returned [deg].
maxwhere the maximum value is returned [deg].
Returns
true/false on success/failure.
bool CartesianBot::getPose ( yarp::sig::Vector &  x,
yarp::sig::Vector &  o,
yarp::os::Stamp *  stamp = NULL 
)
virtual

Get the current pose of the end-effector. [do not wait for reply]

Parameters
xa 3-d vector which is filled with the actual position x,y,z (meters).
oda 2-d vector which is filled with the actual orientation using euler representation xa, ya, za, theta (meters and radians).
stampthe stamp of the encoders employed to compute the pose.
Returns
true/false on success/failure.
bool CartesianBot::getPose ( const int  axis,
yarp::sig::Vector &  x,
yarp::sig::Vector &  o,
yarp::os::Stamp *  stamp = NULL 
)
virtual

Get the current pose of the specified link belonging to the kinematic chain. [wait for reply]

Parameters
axisjoint index (regardless if it is actuated or not).
xa 3-d vector which is filled with the actual position x,y,z (meters) of the given link reference frame.
oda 2-d vector which is filled with the actual orientation of the given link reference frame using axis-angle representation xa, ya, za, theta (meters and radians).
stampthe stamp of the encoders employed to compute the pose.
Returns
true/false on success/failure.
virtual bool CartesianBot::getPosePriority ( yarp::os::ConstString &  p)
inlinevirtual

Get the current pose priority. [wait for reply]

Parameters
phere is returned either as "position" or "orientation".
Returns
true/false on success/failure.
bool CartesianBot::getReferenceMode ( bool *  f)
virtual

Get the current controller reference mode. [wait for reply]

Parameters
fhere is returned true if controller makes use of the low-level joints set-points, false if it employs actual encoders feedback.
Returns
true/false on success/failure.
bool CartesianBot::getRestPos ( yarp::sig::Vector &  curRestPos)
virtual

Get the current joints rest position. [wait for reply]

Parameters
curRestPosa vector which is filled with the current joints rest position components in degrees.
Returns
true/false on success/failure.
Note
While solving the inverse kinematic, the user may specify a secondary task that minimizes against a joints rest position; further, each rest component may be weighted differently providing the weights vector.
bool CartesianBot::getRestWeights ( yarp::sig::Vector &  curRestWeights)
virtual

Get the current joints rest weights. [wait for reply]

Parameters
curRestWeightsa vector which is filled with the current joints rest weights.
Returns
true/false on success/failure.
Note
While solving the inverse kinematic, the user may specify a secondary task that minimizes against a joints rest position; further, each rest component may be weighted differently providing the weights vector.
bool CartesianBot::getTaskVelocities ( yarp::sig::Vector &  xdot,
yarp::sig::Vector &  odot 
)
virtual

Return velocities of the end-effector in the task space. [wait for reply]

Parameters
xdotthe 3-d vector containing the derivative of x,y,z position [m/s] of the end-effector while moving in the task space as result of the commanded joints velocities.
odotthe 2-d vector containing the derivative of end-effector orientation [rad/s] while moving in the task space as result of the commanded joints velocities.
Returns
true/false on success/failure.
bool CartesianBot::getTipFrame ( yarp::sig::Vector &  x,
yarp::sig::Vector &  o 
)
virtual

Retrieve the tip frame currently attached to the end-effector.

Parameters
xa 3-d vector containing the position of the tip frame wrt the end-effector (meters).
oa 4-d vector containing the orientation of the tip frame wrt the end-effector (axis-angle notation).
Returns
true/false if successful/failed.
bool CartesianBot::getTrackingMode ( bool *  f)
virtual

Get the current controller mode. [wait for reply]

Parameters
fhere is returned true if controller is in tracking mode, false otherwise.
Returns
true/false on success/failure.
bool CartesianBot::getTrajTime ( double *  t)
virtual

Get the current trajectory duration. [wait for reply]

Parameters
tthe memory location where the time is returned (seconds).
Returns
true/false on success/failure.
bool CartesianBot::goToPose ( const yarp::sig::Vector &  xd,
const yarp::sig::Vector &  od,
const double  t = 0 
)
virtual

Move the end-effector to a specified pose (position and orientation) in cartesian space. [do not wait for reply]

Parameters
xda 3-d vector which contains the desired position x,y,z
oda 2-d vector which contains the desired orientation using axis-angle representation (xa, ya, za, theta).
tset the trajectory duration time (seconds). If t< (as by default) the current execution time is kept.
Returns
true/false on success/failure.
Note
Intended for streaming mode.
bool CartesianBot::goToPoseSync ( const yarp::sig::Vector &  xd,
const yarp::sig::Vector &  od,
const double  t = 0.0 
)
virtual

Move the end-effector to a specified pose (position and orientation) in cartesian space. [wait for reply]

Parameters
xda 3-d vector which contains the desired position x,y,z (meters).
oda 2-d vector which contains the desired orientation using axis-angle representation (xa, ya, za, theta).
tset the trajectory duration time (seconds). If t<=0 (as by default) the current execution time is kept.
Returns
true/false on success/failure.
bool CartesianBot::goToPosition ( const yarp::sig::Vector &  xd,
const double  t = 0.0 
)
virtual

Move the end-effector to a specified position in cartesian space, ignore the orientation. [do not wait for reply]

Parameters
xda 3-d vector which contains the desired position x,y,z (meters).
tset the trajectory duration time (seconds). If t<=0 (as by default) the current execution time is kept.
Returns
true/false on success/failure.
Note
Intended for streaming mode.
bool CartesianBot::goToPositionSync ( const yarp::sig::Vector &  xd,
const double  t = 0.0 
)
virtual

Move the end-effector to a specified position in cartesian space, ignore the orientation. [wait for reply]

Parameters
xda 3-d vector which contains the desired position x,y,z (meters).
tset the trajectory duration time (seconds). If t<=0 (as by default) the current execution time is kept.
Returns
true/false on success/failure.
bool CartesianBot::open ( Searchable &  config)
virtual

Open the DeviceDriver.

Parameters
configis a list of parameters for the device. Which parameters are effective for your device can vary. See device invocation examples. If there is no example for your device, you can run the "yarpdev" program with the verbose flag set to probe what parameters the device is checking. If that fails too, you'll need to read the source code (please nag one of the yarp developers to add documentation for your device).
Returns
true/false upon success/failure
bool CartesianBot::registerEvent ( yarp::dev::CartesianEvent &  event)
virtual

Register an event.

Parameters
eventthe event to be registered.
Returns
true/false on success/failure.
Note
the special type "*" can be used to attach a callback to all the available events.
bool CartesianBot::removeTipFrame ( )
virtual

Remove the tip frame currently attached to the end-effector.

Returns
true/false if successful/failed.
Note
The actual end-effector is again under control.
bool CartesianBot::restoreContext ( const int  id)
virtual

Restore the controller context previously stored. [wait for reply]

Parameters
idspecify the context id to be restored
Returns
true/false on success/failure.
Note
The context comprises the values of internal controller variables, such as the tracking mode, the active dofs, the trajectory time and so on.
void CartesianBot::run ( )

Loop function. This is the thread itself.

bool CartesianBot::setDOF ( const yarp::sig::Vector &  newDof,
yarp::sig::Vector &  curDof 
)
virtual

Set a new DOF configuration for the limb. [wait for reply]

Parameters
newDofa vector which contains the new DOF configuration.
curDofa vector where the DOF configuration is returned as it has been processed after the request (it may differ from newDof due to the presence of some internal limb's constraints).
Returns
true/false on success/failure.
Note
Eeach vector's position shall contain 1 if the associated joint can be actuated, 0 otherwise. The special value 2 indicates that the joint status won't be modified (useful as a placeholder).
bool CartesianBot::setInTargetTol ( const double  tol)
virtual

Set tolerance for in-target check. [wait for reply]

Parameters
toltolerance.
Returns
true/false on success/failure.
Note
The trajectory is supposed to be completed as soon as norm(xd-end_effector)<tol.
bool CartesianBot::setLimits ( const int  axis,
const double  min,
const double  max 
)
virtual

Set new range for the axis. Allowed range shall be a valid subset of the real control limits. [wait for reply]

Parameters
axisjoint index (regardless it it is actuated or not).
minthe new minimum value [deg].
maxthe new maximum value [deg].
Returns
true/false on success/failure.
virtual bool CartesianBot::setPosePriority ( const yarp::os::ConstString &  p)
inlinevirtual

Ask the controller to weigh more either the position or the orientation while reaching in full pose. [wait for reply]

Parameters
pcan be "position" or "orientation".
Returns
true/false on success/failure.
bool CartesianBot::setReferenceMode ( const bool  f)
virtual

Ask the controller to close the loop with the low-level joints set-points in place of the actual encoders feedback. [wait for reply]

Parameters
ftrue for reference mode, false otherwise.
Returns
true/false on success/failure.
Note
When the reference mode is enabled the controller makes use of the low-level joints set-points that result from the integration of the velocity commands in place of the actual encoders feedbacks. This modality is particularly useful in a scenario where the velocity commands are executed by the control boards with resort to torque actuation.
bool CartesianBot::setRestPos ( const yarp::sig::Vector &  newRestPos,
yarp::sig::Vector &  curRestPos 
)
virtual

Set a new joints rest position. [wait for reply]

Parameters
newRestPosa vector which contains the new joints rest position components in degrees.
curRestPosa vector which is filled with the current joints rest position components in degrees as result from thresholding with joints bounds.
Returns
true/false on success/failure.
Note
While solving the inverse kinematic, the user may specify a secondary task that minimizes against a joints rest position; further, each rest component may be weighted differently providing the weights vector.
bool CartesianBot::setRestWeights ( const yarp::sig::Vector &  newRestWeights,
yarp::sig::Vector &  curRestWeights 
)
virtual

Set a new joints rest position. [wait for reply]

Parameters
newRestWeightsa vector which contains the new joints rest weights.
curRestWeightsa vector which is filled with the current joints rest weights as result from saturation (w>=0.0).
Returns
true/false on success/failure.
Note
While solving the inverse kinematic, the user may specify a secondary task that minimizes against a joints rest position; further, each rest component may be weighted differently providing the weights vector.
bool CartesianBot::setTaskVelocities ( const yarp::sig::Vector &  xdot,
const yarp::sig::Vector &  odot 
)
virtual

Set the reference velocities of the end-effector in the task space.

Parameters
xdotthe 3-d vector containing the x,y,z reference velocities [m/s] of the end-effector.
odotthe 2-d vector containing the orientation reference velocity [rad/s] of the end-effector
Returns
true/false on success/failure.
bool CartesianBot::setTrackingMode ( const bool  f)
virtual

Set the controller in tracking or non-tracking mode. [wait for reply]

Parameters
ftrue for tracking mode, false otherwise.
Returns
true/false on success/failure.
Note
In tracking mode when the controller reaches the target, it keeps on running in order to maintain the limb in the desired pose in the presence of external disturbances. In non-tracking mode the controller releases the limb as soon as the desired pose is reached.
bool CartesianBot::setTrajTime ( const double  t)
virtual

Set the duration of the trajectory. [wait for reply]

Parameters
ttime (seconds).
Returns
true/false on success/failure.
bool CartesianBot::stopControl ( )
virtual

Ask for an immediate stop motion. [wait for reply]

Returns
true/false on success/failure.
Note
The control is completely released, i.e. a direct switch to non-tracking mode is executed.
bool CartesianBot::storeContext ( int *  id)
virtual

Store the controller context. [wait for reply]

Parameters
idspecify where to store the returned context id.
Returns
true/false on success/failure.
Note
The context comprises the values of internal controller variables, such as the tracking mode, the active dofs, the trajectory time and so on.
bool CartesianBot::threadInit ( )

Initialization method. The thread executes this function when it starts and before "run". This is a good place to perform initialization tasks that need to be done by the thread itself (device drivers initialization, memory allocation etc). If the function returns false the thread quits and never calls "run". The return value of threadInit() is notified to the class and passed as a parameter to afterStart(). Note that afterStart() is called by the same thread that is executing the "start" method.

bool CartesianBot::tweakGet ( yarp::os::Bottle &  options)
virtual

Return low-level controller's parameters. [wait for reply]

Parameters
optionsis a property-like bottle containing the current values of the low-level controller's configuration.
Returns
true/false on success/failure.
Note
This method is intended for accessing low-level controller's configuration.
bool CartesianBot::tweakSet ( const yarp::os::Bottle &  options)
virtual

Tweak low-level controller's parameters. [wait for reply]

Parameters
optionsis a property-like bottle containing new values for the low-level controller's configuration.
Returns
true/false on success/failure.
Note
This method is intended for accessing low-level controller's configuration.
bool CartesianBot::unregisterEvent ( yarp::dev::CartesianEvent &  event)
virtual

Unregister an event.

Parameters
eventthe event to be unregistered.
Returns
true/false on success/failure.
bool CartesianBot::waitMotionDone ( const double  period = 0.1,
const double  timeout = 0.0 
)
virtual

Wait until the current trajectory is terminated. [wait for reply]

Parameters
periodspecify the check time period (seconds).
timeoutspecify the check expiration time (seconds). If timeout<=0 (as by default) the check will be performed without time limitation.
Returns
true for success, false for failure and timeout expired.

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